示例#1
0
void SBANode::doSBA(/*const ros::TimerEvent& event*/)
{
  unsigned int projs = 0;
  // For debugging.
  for (int i = 0; i < (int)sba.tracks.size(); i++)
  {
    projs += sba.tracks[i].projections.size();
  }
  ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sba.nodes.size(),
    (int)sba.tracks.size(), projs);
  
  if (sba.nodes.size() > 0)
  {
    // Copied from vslam.cpp: refine()
    sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);
    
    double cost = sba.calcRMSCost();
    
    if (isnan(cost) || isinf(cost)) // is NaN?
    {
      ROS_INFO("NaN cost!");  
    }
    else
    { 
      if (sba.calcRMSCost() > 4.0)
        sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);  // do more
      if (sba.calcRMSCost() > 4.0)
        sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);  // do more
    }
  }
}
int main(int argc, char **argv)
{
  char *fin;

  if (argc < 2)
    {
      cout << "Arguments are:  <input filename>" << endl;
      return -1;
    }

  fin = argv[1];

  SysSBA sys;
  readGraphFile(fin, sys);
  //  writeGraphFile("sba-out.graph", sys);

  double cost = sys.calcCost();
  cout << "Initial squared cost: " << cost << endl;

  sys.nFixed = 1;
  sys.printStats();
  sys.csp.useCholmod = true;

  //  sys.doSBA(10,1e-4,SBA_SPARSE_CHOLESKY);
  sys.doSBA(10,1e-4,SBA_BLOCK_JACOBIAN_PCG,1e-8,200);

  return 0;
}
void processSBA(ros::NodeHandle node)
{
    // Create publisher topics.
    ros::Publisher cam_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
    ros::Publisher point_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/points", 1);
    ros::spinOnce();
    
    //ROS_INFO("Sleeping for 2 seconds to publish topics...");
    ros::Duration(0.2).sleep();
    
    // Create an empty SBA system.
    SysSBA sys;
    
    setupSBA(sys);
    
    // Provide some information about the data read in.
    unsigned int projs = 0;
    // For debugging.
    for (int i = 0; i < (int)sys.tracks.size(); i++)
    {
      projs += sys.tracks[i].projections.size();
    }
    ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sys.nodes.size(),
      (int)sys.tracks.size(), projs);
        
    //ROS_INFO("Sleeping for 5 seconds to publish pre-SBA markers.");
    //ros::Duration(5.0).sleep();
        
    // Perform SBA with 10 iterations, an initial lambda step-size of 1e-3, 
    // and using CSPARSE.
    sys.doSBA(20, 1e-4, SBA_SPARSE_CHOLESKY);
    
    int npts = sys.tracks.size();

    ROS_INFO("Bad projs (> 10 pix): %d, Cost without: %f", 
        (int)sys.countBad(10.0), sqrt(sys.calcCost(10.0)/npts));
    ROS_INFO("Bad projs (> 5 pix): %d, Cost without: %f", 
        (int)sys.countBad(5.0), sqrt(sys.calcCost(5.0)/npts));
    ROS_INFO("Bad projs (> 2 pix): %d, Cost without: %f", 
        (int)sys.countBad(2.0), sqrt(sys.calcCost(2.0)/npts));
    
    ROS_INFO("Cameras (nodes): %d, Points: %d",
        (int)sys.nodes.size(), (int)sys.tracks.size());
        
    // Publish markers
    drawGraph(sys, cam_marker_pub, point_marker_pub);
    ros::spinOnce();
    //ROS_INFO("Sleeping for 2 seconds to publish post-SBA markers.");
    ros::Duration(0.2).sleep();
}
示例#4
0
  // assumes cv::Mats are of type <float>
  int PoseEstimator::estimate(const matches_t &matches, 
                              const cv::Mat &train_kpts, const cv::Mat &query_kpts,
                              const cv::Mat &train_pts, const cv::Mat &query_pts,
			      const cv::Mat &K, const double baseline)
  {
    // make sure we're using floats
    if (train_kpts.depth() != CV_32F ||
	query_kpts.depth() != CV_32F ||
	train_pts.depth() != CV_32F ||
	query_pts.depth() != CV_32F)
      throw std::runtime_error("Expected input to be of floating point (CV_32F)");

    int nmatch = matches.size();

    cout << endl << "[pe3d] Matches: " << nmatch << endl;

    // set up data structures for fast processing
    // indices to good matches

    std::vector<Eigen::Vector3f> t3d, q3d;
    std::vector<Eigen::Vector2f> t2d, q2d;
    std::vector<int> m;		// keep track of matches 

    for (int i=0; i<nmatch; i++)
      {
	const float* ti = train_pts.ptr<float>(matches[i].trainIdx);
	const float* qi = query_pts.ptr<float>(matches[i].queryIdx);
	const float* ti2 = train_kpts.ptr<float>(matches[i].trainIdx);
	const float* qi2 = query_kpts.ptr<float>(matches[i].queryIdx);

	// make sure we have points within range; eliminates NaNs as well
        if (matches[i].distance < 60 && // TODO: get this out of the estimator
	    ti[2] < maxMatchRange && 
	    qi[2] < maxMatchRange)
	  {
	    m.push_back(i);
	    t2d.push_back(Eigen::Vector2f(ti2[0],ti2[1]));
	    q2d.push_back(Eigen::Vector2f(qi2[0],qi2[1]));
	    t3d.push_back(Eigen::Vector3f(ti[0],ti[1],ti[2]));
	    q3d.push_back(Eigen::Vector3f(qi[0],qi[1],qi[2]));
          }
      }


    nmatch = m.size();
    cout << "[pe3d] Filtered matches: " << nmatch << endl;

    if (nmatch < 3) return 0;   // can't do it...

    int bestinl = 0;
    Matrix3f Kf;
    cv::cv2eigen(K,Kf);		    // camera matrix
    float fb = Kf(0,0)*baseline; // focal length times baseline
    float maxInlierXDist2 = maxInlierXDist*maxInlierXDist;
    float maxInlierDDist2 = maxInlierDDist*maxInlierDDist;

    // set up minimum hyp pixel distance
    float minpdist = minPDist * Kf(0,2) * 2.0;

    // RANSAC loop
    int numchoices = 1 + numRansac / 10;
    for (int ii=0; ii<numRansac; ii++) 
      {
        // find a candidate
	int k;
        int a=rand()%nmatch;
	int b;
	for (k=0; k<numchoices; k++)
	  {
	    b=rand()%nmatch;
	    if (a!=b && (t2d[a]-t2d[b]).norm() > minpdist
		     && (q2d[a]-q2d[b]).norm() > minpdist)
	      // TODO: add distance check
	      break;
	  }
	if (k >= numchoices) continue;
        int c;
	for (k=0; k<numchoices+numchoices; k++)
	  {
	    c=rand()%nmatch;
	    if (c!=b && c!=a && (t2d[a]-t2d[c]).norm() > minpdist
		&& (t2d[b]-t2d[c]).norm() > minpdist
		&& (q2d[a]-q2d[c]).norm() > minpdist
		&& (q2d[b]-q2d[c]).norm() > minpdist)
	      // TODO: add distance check
	      break;
	  }
	if (k >= numchoices+numchoices) continue;

        // get centroids
        Vector3f p0a = t3d[a];
        Vector3f p0b = t3d[b];
        Vector3f p0c = t3d[c];

        Vector3f p1a = q3d[a];
        Vector3f p1b = q3d[b];
        Vector3f p1c = q3d[c];

        Vector3f c0 = (p0a+p0b+p0c)*(1.0/3.0);
        Vector3f c1 = (p1a+p1b+p1c)*(1.0/3.0);

        // subtract out
        p0a -= c0;
        p0b -= c0;
        p0c -= c0;
        p1a -= c1;
        p1b -= c1;
        p1c -= c1;

        Matrix3f Hf = p1a*p0a.transpose() + p1b*p0b.transpose() +
	              p1c*p0c.transpose();
	Matrix3d H = Hf.cast<double>();

#if 0
        cout << p0a.transpose() << endl;
        cout << p0b.transpose() << endl;
        cout << p0c.transpose() << endl;
#endif

        // do the SVD thang
        JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
        Matrix3d V = svd.matrixV();
        Matrix3d R = V * svd.matrixU().transpose();          
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
          {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
          }

	Vector3d cd0 = c0.cast<double>();
	Vector3d cd1 = c1.cast<double>();
        Vector3d tr = cd0-R*cd1;    // translation 

	//      cout << "[PE test] R: " << endl << R << endl;
	//	cout << "[PE test] t: " << tr.transpose() << endl;

        Vector3f trf = tr.cast<float>();      // convert to floats
        Matrix3f Rf = R.cast<float>();
	Rf = Kf*Rf;
	trf = Kf*trf;

        // find inliers, based on image reprojection
        int inl = 0;
        for (int i=0; i<nmatch; i++)
          {
            const Vector3f &pt1 = q3d[i];
            Vector3f ipt = Rf*pt1+trf; // transform query point
            float iz = 1.0/ipt.z();
	    Vector2f &kp = t2d[i];
	    //	    cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
            float dx = kp.x() - ipt.x()*iz;
            float dy = kp.y() - ipt.y()*iz;
            float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2
                && dd*dd < maxInlierDDist2)
               //              inl+=(int)fsqrt(ipt.z()); // clever way to weight closer points
              inl++;
          }
        
        if (inl > bestinl) 
          {
            bestinl = inl;
            trans = tr.cast<float>();      // convert to floats
            rot = R.cast<float>();
          }

      }

    cout << "[pe3d] Best inliers: " << bestinl << endl;
//    printf("Total ransac: %d  Neg det: %d\n", ntot, nneg);

    // reduce matches to inliers
    matches_t inls;    // temporary for current inliers
    inliers.clear();
    Matrix3f Rf = Kf*rot;
    Vector3f trf = Kf*trans;

    //    cout << "[pe3e] R: " << endl << rot << endl;
    cout << "[pe3d] t: " << trans.transpose() << endl;

    AngleAxisf aa;
    aa.fromRotationMatrix(rot);
    cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << "   " << aa.axis().transpose() << endl;

    for (int i=0; i<nmatch; i++)
      {
	Vector3f &pt1 = q3d[i];
        Vector3f ipt = Rf*pt1+trf; // transform query point
        float iz = 1.0/ipt.z();
	Vector2f &kp = t2d[i];
	//	cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
        float dx = kp.x() - ipt.x()*iz;
        float dy = kp.y() - ipt.y()*iz;
        float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d

        if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 && 
            dd*dd < maxInlierDDist2)
	  inls.push_back(matches[m[i]]); 
      }

    cout << "[pe3d] Final inliers: " << inls.size() << endl;

    // polish with SVD
    if (polish)
      {
	Matrix3d Rd = rot.cast<double>();
	Vector3d trd = trans.cast<double>();
	StereoPolish pol(5,false);
	pol.polish(inls,train_kpts,query_kpts,train_pts,query_pts,K,baseline,
	   Rd,trd);

	AngleAxisd aa;
	aa.fromRotationMatrix(Rd);
	cout << "[pe3d] Polished t: " << trd.transpose() << endl;
	cout << "[pe3d] Polished AA: " << aa.angle()*180.0/M_PI << "   " << aa.axis().transpose() << endl;

	int num = inls.size();
	// get centroids
	Vector3f c0(0,0,0);
	Vector3f c1(0,0,0);
	for (int i=0; i<num; i++)
	  {
	    c0 += t3d[i];
	    c1 += q3d[i];
	  }

	c0 = c0 / (float)num;
	c1 = c1 / (float)num;

        // subtract out and create H matrix
	Matrix3f Hf;
	Hf.setZero();

	for (int i=0; i<num; i++)
	  {
	    Vector3f p0 = t3d[i]-c0;
	    Vector3f p1 = q3d[i]-c1;
	    Hf += p1*p0.transpose();
	  }

	Matrix3d H = Hf.cast<double>();

        // do the SVD thang
        JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
        Matrix3d V = svd.matrixV();
        Matrix3d R = V * svd.matrixU().transpose();          
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
          {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
          }

	Vector3d cd0 = c0.cast<double>();
	Vector3d cd1 = c1.cast<double>();
        Vector3d tr = cd0-R*cd1;    // translation 


	aa.fromRotationMatrix(R);
	cout << "[pe3d] t: " << tr.transpose() << endl;
	cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << "   " << aa.axis().transpose() << endl;

#if 0
        // system
        SysSBA sba;
        sba.verbose = 0;

        // set up nodes
        // should have a frame => node function        
        Vector4d v0 = Vector4d(0,0,0,1);
        Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
        sba.addNode(v0, q0, f0.cam, true);
        
        Quaterniond qr1(rot);   // from rotation matrix
        Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);

        //        sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
        qr1.normalize();
        sba.addNode(temptrans, qr1, f1.cam, false);

        int in = 3;
        if (in > (int)inls.size())
          in = inls.size();

        // set up projections
        for (int i=0; i<(int)inls.size(); i++)
          {
            // add point
            int i0 = inls[i].queryIdx;
            int i1 = inls[i].trainIdx;
            Vector4d pt = query_pts[i0];
            sba.addPoint(pt);
            
            // projected point, ul,vl,ur
            Vector3d ipt;
            ipt(0) = f0.kpts[i0].pt.x;
            ipt(1) = f0.kpts[i0].pt.y;
            ipt(2) = ipt(0)-f0.disps[i0];
            sba.addStereoProj(0, i, ipt);

            // projected point, ul,vl,ur
            ipt(0) = f1.kpts[i1].pt.x;
            ipt(1) = f1.kpts[i1].pt.y;
            ipt(2) = ipt(0)-f1.disps[i1];
            sba.addStereoProj(1, i, ipt);
          }

        sba.huber = 2.0;
        sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
        int nbad = sba.removeBad(2.0);
//        cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
        sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);

//        cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;

        // get the updated transform
	      trans = sba.nodes[1].trans.head(3);
	      Quaterniond q1;
	      q1 = sba.nodes[1].qrot;
	      rot = q1.toRotationMatrix();

        // set up inliers
        inliers.clear();
        for (int i=0; i<(int)inls.size(); i++)
          {
            ProjMap &prjs = sba.tracks[i].projections;
            if (prjs[0].isValid && prjs[1].isValid) // valid track
              inliers.push_back(inls[i]);
          }
#if 0
        printf("Inliers: %d   After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif

#endif

      }

    inliers = inls;
    return (int)inls.size();
  }
示例#5
0
// test the transform functions
TEST(SBAtest, SimpleSystem)
{
  // set up full system
  SysSBA sys;
  
  // set of world points
  // each row is a point
  Matrix<double,23,4> pts;
  pts << 0.5,  0.2, 3,   1.0,
         1,    0,   2,   1.0,
        -1,    0,   2,   1.0,
         0,    0.2, 3,   1.0,
         1,    1,   2,   1.0,
        -1,   -1,   2,   1.0,
         1,    0.2, 4,   1.0,
         0,    1,   2.5, 1.0,
         0,   -1,   2.5, 1.0,
         0.2,  0,   3,   1.0,
        -1,    1,   2.5, 1.0,
         1,   -1,   2.5, 1.0,
         0.5,  0.2, 4,   1.0,
         0.2, -1.3, 2.5, 1.0,
        -0.5, -1,   2.5, 1.0,
         0.2, -0.7, 3,   1.0,
        -1,    1,   3.5, 1.0,
         1,   -1,   3.5, 1.0,
         0.5,  0.2, 4.6, 1.0,
        -1,    0,   4,   1.0,
         0,    0,   4,   1.0,
         1,    1,   4,   1.0,
        -1,   -1,   4,   1.0;

  for (int i=0; i<pts.rows(); i++)
    {
      Point pt(pts.row(i));
      sys.addPoint(pt);
    }

  Node::initDr();               // set up fixed matrices

  // set of nodes
  // three nodes, one at origin, two displaced
  CamParams cpars = {300,300,320,240,0.1}; // 300 pix focal length, 0.1 m baseline

  Quaternion<double> frq2(AngleAxisd(10*M_PI/180,Vector3d(.2,.3,.4).normalized())); // frame rotation in the world
  Vector4d frt2(0.2, -0.4, 1.0, 1.0); // frame position in the world
  Quaternion<double> frq3(AngleAxisd(10*M_PI/180,Vector3d(-.2,.1,-.3).normalized())); // frame rotation in the world
  Vector4d frt3(-0.2, 0.4, 1.0, 1.0); // frame position in the world
  Vector3d b(cpars.tx,0,0);

  // set up nodes
  Node nd1;
  Vector4d qr(0,0,0,1);
  nd1.qrot = qr;		// no rotation 
  nd1.trans = Vector4d::Zero();	// or translation
  nd1.setTransform();		// set up world2node transform
  nd1.setKcam(cpars);		// set up node2image projection
#ifdef LOCAL_ANGLES
  nd1.setDr(true);              // set rotational derivatives
#else
  nd1.setDr(false);             // set rotational derivatives
#endif
  nd1.isFixed = true;

  Node nd2;
  nd2.qrot = frq2;
  cout << "Quaternion: " << nd2.qrot.coeffs().transpose() << endl;
  nd2.trans = frt2;
  cout << "Translation: " << nd2.trans.transpose() << endl << endl;
  nd2.setTransform();		// set up world2node transform
  nd2.setKcam(cpars);		// set up node2image projection
#ifdef LOCAL_ANGLES
  nd2.setDr(true);              // set rotational derivatives
#else
  nd2.setDr(false);             // set rotational derivatives
#endif
  nd2.isFixed = false;

  Node nd3;
  nd3.qrot = frq3;	
  cout << "Quaternion: " << nd3.qrot.coeffs().transpose() << endl;
  nd3.trans = frt3;
  cout << "Translation: " << nd3.trans.transpose() << endl << endl;
  nd3.setTransform();		// set up world2node transform
  nd3.setKcam(cpars);		// set up node2image projection
#ifdef LOCAL_ANGLES
  nd3.setDr(true);              // set rotational derivatives
#else
  nd3.setDr(false);             // set rotational derivatives
#endif
  nd3.isFixed = false;

  sys.nodes.push_back(nd1);
  sys.nodes.push_back(nd2);
  sys.nodes.push_back(nd3);

  // set up projections onto nodes
  int ind = 0;
  double inoise = 0.5;
  Vector3d n2;

  for(unsigned int i = 0; i < sys.tracks.size(); i++)
    {
      Point pt = sys.tracks[i].point;      
      ProjMap &prjs = sys.tracks[i].projections;	// new point track
      Proj prj;
      prj.isValid = true;
      prj.stereo = true;
      Vector2d ipt;
      Vector3d ipt3,pc;

      n2.setRandom();
      nd1.project2im(ipt,pt); // set up projection measurement
      prj.ndi = 0;		// nd1 index
      ipt3.head(2) = ipt;
      pc = nd1.Kcam * (nd1.w2n*pt - b); // camera coords for right cam
      ipt3(2) = pc(0)/pc(2);
      prj.kp = ipt3 + n2*inoise;
      prjs[prjs.size()] = prj;

      n2.setRandom();
      nd2.project2im(ipt,pt);	// set up projection measurement
      prj.ndi = 1;		// nd2 index
      ipt3.head(2) = ipt;
      pc = nd2.Kcam * (nd2.w2n*pt - b); // camera coords for right cam
      ipt3(2) = pc(0)/pc(2);
      prj.kp = ipt3 + n2*inoise;
      prjs[prjs.size()] = prj;

      n2.setRandom();
      nd3.project2im(ipt,pt);	// set up projection measurement
      prj.ndi = 2;		// nd3 index
      ipt3.head(2) = ipt;
      pc = nd3.Kcam * (nd3.w2n*pt - b); // camera coords for right cam
      ipt3(2) = pc(0)/pc(2);
      prj.kp = ipt3 + n2*inoise;
      prjs[prjs.size()] = prj;

      //sys.tracksSt.push_back(prjs);
      ind++;
    }


#ifdef WRITE_GRAPH
  writeGraphFile("sba.graph",sys);
#endif

  double qnoise = 10*M_PI/180;	// in radians
  double tnoise = 0.1;		// in meters

  // add random noise to node positions
  nd2.qrot.coeffs().head<3>() += qnoise*Vector3d::Random();
  nd2.normRot();
  cout << "Quaternion: " << nd2.qrot.coeffs().transpose() << endl << endl;
  nd2.trans.head<3>() += tnoise*Vector3d::Random();
  nd2.setTransform();		// set up world2node transform
  nd2.setProjection();
#ifdef LOCAL_ANGLES
  nd2.setDr(true);              // set rotational derivatives
#else
  nd2.setDr(false);              // set rotational derivatives
#endif
  sys.nodes[1] = nd2;		// reset node
  
  nd3.qrot.coeffs().head<3>() += qnoise*Vector3d::Random();
  nd3.normRot();
  //  cout << "Quaternion: " << nd3.qrot.transpose() << endl << endl;
  nd3.trans.head<3>() += tnoise*Vector3d::Random();
  nd3.setTransform();		// set up world2node transform
  nd3.setProjection();		// set up node2image projection
#ifdef LOCAL_ANGLES
  nd3.setDr(true);              // set rotational derivatives
#else
  nd3.setDr(false);             // set rotational derivatives
#endif
  sys.nodes[2] = nd3;		// reset node

#ifdef WRITE_GRAPH
  writeGraphFile("sba-with-err.graph",sys);
#endif

#if 0
  // set up system, no lambda for here
  sys.setupSys(0.0);
  ofstream(fd);
  fd.open("A.txt");
  fd.precision(8);		// this is truly inane
  fd << fixed;
  fd << sys.A << endl;
  fd.close();
  fd.open("B.txt");
  fd.precision(8);
  fd << fixed;
  fd << sys.B << endl;
  fd.close();
#endif
  
#ifndef LOCAL_ANGLES
  sys.useLocalAngles = false;
#endif

  sys.nFixed = 1;		// number of fixed cameras
  sys.doSBA(10,1.0e-3,false);

  cout << endl << "Quaternion: " << sys.nodes[1].qrot.coeffs().transpose() << endl;
  // normalize output translation
  Vector4d frt2a = sys.nodes[1].trans;
  double s = frt2.head<3>().norm() / frt2a.head<3>().norm();
  frt2a.head<3>() *= s;
  cout << "Translation: " << frt2a.transpose() << endl << endl;

  cout << "Quaternion: " << sys.nodes[2].qrot.coeffs().transpose() << endl;
  Vector4d frt3a = sys.nodes[2].trans;
  s = frt3.head<3>().norm() / frt3a.head<3>().norm();
  frt3a.head<3>() *= s;
  cout << "Translation: " << frt3a.transpose() << endl << endl;

  // calculate cost, should be close to zero
  double cost = 0.0;
  EXPECT_EQ_ABS(cost,0.0,10.0);
  // cameras should be close to their original positions,
  //   adjusted for scale on translations
  for (int i=0; i<4; i++)
    EXPECT_EQ_ABS(sys.nodes[1].qrot.coeffs()[i],frq2.coeffs()[i],0.01);
  for (int i=0; i<4; i++)
    EXPECT_EQ_ABS(sys.nodes[2].qrot.coeffs()[i],frq3.coeffs()[i],0.01);
  for (int i=0; i<3; i++)
    EXPECT_EQ_ABS(frt2a(i),frt2(i),0.01);
  for (int i=0; i<3; i++)
    EXPECT_EQ_ABS(frt3a(i),frt3(i),0.01);

#if 1
  // writing out matrices, 3-node system
  // global system
  sys.useLocalAngles = false;
  nd1.setDr(false);
  nd2.setDr(false);
  nd3.setDr(false);
  sys.setupSys(0.0);
  sys.A.block<6,6>(6,0) = sys.A.block<6,6>(0,6).transpose();
  writeA("A3g.txt",sys);

  // local system
  sys.useLocalAngles = true;
  nd1.setDr(true);
  nd2.setDr(true);
  nd3.setDr(true);
  sys.setupSys(0.0);
  sys.A.block<6,6>(6,0) = sys.A.block<6,6>(0,6).transpose();
  writeA("A3l.txt",sys);
#endif 

#if 0
  // set up 2-node system
  sys.nodes.clear();
  sys.tracks.clear();

  sys.nodes.push_back(nd1);
  sys.nodes.push_back(nd2);

  // set up projections onto nodes
  ind = 0;
  for(vector<Point,Eigen::aligned_allocator<Point> >::iterator itr = sys.points.begin(); itr!=sys.points.end(); itr++)
    {
      Point pt = *itr;      
      vector<Proj,Eigen::aligned_allocator<Proj> > prjs;	// new point track
      Proj prj;
      prj.isValid = true;
      prj.pti = ind;
      prj.kpi = ind;
      Vector2d ipt;

      n2.setRandom();
      nd1.project2im(ipt,pt);	// set up projection measurement
      prj.ndi = 0;		// nd1 index
      prj.kp = ipt + n2*inoise;
      prjs.push_back(prj);

      n2.setRandom();
      nd2.project2im(ipt,pt);	// set up projection measurement
      prj.ndi = 1;		// nd2 index
      prj.kp = ipt + n2*inoise;
      prjs.push_back(prj);

      sys.tracks.push_back(prjs);
      ind++;
    }

  sys.doSBA(3);

  // writing out matrices, 2-node system
  // global system
  sys.useLocalAngles = false;
  nd1.setDr(false);
  nd2.setDr(false);
  sys.setupSys(0.0);
  sys.writeA("A2g.txt");

  // local system
  sys.useLocalAngles = true;
  nd1.setDr(true);
  nd2.setDr(true);
  sys.setupSys(0.0);
  sys.writeA("A2l.txt");
#endif

}
void processSBA(ros::NodeHandle node)
{
    // Create publisher topics.
    ros::Publisher cam_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
    ros::Publisher point_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/points", 1);
    ros::spinOnce();
    
    //ROS_INFO("Sleeping for 2 seconds to publish topics...");
    ros::Duration(0.2).sleep();
    
    // Create an empty SBA system.
    SysSBA sys;
    
    setupSBA(sys);
    
    // Provide some information about the data read in.
    unsigned int projs = 0;
    // For debugging.
    for (int i = 0; i < (int)sys.tracks.size(); i++)
    {
      projs += sys.tracks[i].projections.size();
    }
    ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sys.nodes.size(),
      (int)sys.tracks.size(), projs);
        
    //ROS_INFO("Sleeping for 5 seconds to publish pre-SBA markers.");
    //ros::Duration(5.0).sleep();
        
    // Perform SBA with 10 iterations, an initial lambda step-size of 1e-3, 
    // and using CSPARSE.
    sys.doSBA(1, 1e-3, SBA_SPARSE_CHOLESKY);
    
    int npts = sys.tracks.size();

    ROS_INFO("Bad projs (> 10 pix): %d, Cost without: %f", 
        (int)sys.countBad(10.0), sqrt(sys.calcCost(10.0)/npts));
    ROS_INFO("Bad projs (> 5 pix): %d, Cost without: %f", 
        (int)sys.countBad(5.0), sqrt(sys.calcCost(5.0)/npts));
    ROS_INFO("Bad projs (> 2 pix): %d, Cost without: %f", 
        (int)sys.countBad(0.5), sqrt(sys.calcCost(2.0)/npts));
    
    ROS_INFO("Cameras (nodes): %d, Points: %d",
        (int)sys.nodes.size(), (int)sys.tracks.size());
        
    // Publish markers
    drawGraph(sys, cam_marker_pub, point_marker_pub, 1, sys.tracks.size()/2);
    ros::spinOnce();


    //ROS_INFO("Sleeping for 2 seconds to publish post-SBA markers.");
    ros::Duration(0.5).sleep();

    for (int j=0; j<30; j+=3)
      {
        if (!ros::ok())
	        break;
	      sys.doSBA(1, 0, SBA_SPARSE_CHOLESKY);
	      drawGraph(sys, cam_marker_pub, point_marker_pub, 1, sys.tracks.size()/2);
	      ros::spinOnce();
	      ros::Duration(0.5).sleep();
      }


#ifdef USE_PPL
    // reset covariances and continue
    for (int i = 0; i < points.size(); i++)
    {
      int nn = points.size();
      Matrix3d covar;
      double cv = 0.3;
      covar << cv, 0, 0,
	0, cv, 0, 
	0, 0, cv;
      sys.setProjCovariance(0, i+nn, covar);
      sys.setProjCovariance(1, i, covar);
    }
#endif

    for (int j=0; j<30; j+=3)
      {
        if (!ros::ok())
	        break;
	      sys.doSBA(1, 0, SBA_SPARSE_CHOLESKY);
	      drawGraph(sys, cam_marker_pub, point_marker_pub, 1, sys.tracks.size()/2);
	      ros::spinOnce();
	      ros::Duration(0.5).sleep();
      }

}
示例#7
0
  int PoseEstimator3d::estimate(const Frame& f0, const Frame& f1, 
                                const std::vector<cv::DMatch> &matches)
  {
    // convert keypoints in match to 3d points
    std::vector<Vector4d, aligned_allocator<Vector4d> > p0; // homogeneous coordinates
    std::vector<Vector4d, aligned_allocator<Vector4d> > p1;

    int nmatch = matches.size();

    // set up data structures for fast processing
    // indices to good matches
    vector<int> m0, m1;
    for (int i=0; i<nmatch; i++)
      {
        if (f0.disps[matches[i].queryIdx] > minMatchDisp && 
            f1.disps[matches[i].trainIdx] > minMatchDisp)
          {
            m0.push_back(matches[i].queryIdx);
            m1.push_back(matches[i].trainIdx);
          }
      }

    nmatch = m0.size();
    if (nmatch < 3) return 0;   // can't do it...

    int bestinl = 0;

    // RANSAC loop
    #pragma omp parallel for shared( bestinl )
    for (int i=0; i<numRansac; i++) 
      {
        // find a candidate
        int a=rand()%nmatch;
        int b = a;
        while (a==b)
          b=rand()%nmatch;
        int c = a;
        while (a==c || b==c)
          c=rand()%nmatch;

        int i0a = m0[a];
        int i0b = m0[b];
        int i0c = m0[c];
        int i1a = m1[a];
        int i1b = m1[b];
        int i1c = m1[c];

        if (i0a == i0b || i0a == i0c || i0b == i0c ||
            i1a == i1b || i1a == i1c || i1b == i1c)
          continue;

        // get centroids
        Vector3d p0a = f0.pts[i0a].head<3>();
        Vector3d p0b = f0.pts[i0b].head<3>();
        Vector3d p0c = f0.pts[i0c].head<3>();
        Vector3d p1a = f1.pts[i1a].head<3>();
        Vector3d p1b = f1.pts[i1b].head<3>();
        Vector3d p1c = f1.pts[i1c].head<3>();

        Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
        Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);

        // subtract out
        p0a -= c0;
        p0b -= c0;
        p0c -= c0;
        p1a -= c1;
        p1b -= c1;
        p1c -= c1;

        Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
                     p1c*p0c.transpose();

#if 0
        cout << p0a.transpose() << endl;
        cout << p0b.transpose() << endl;
        cout << p0c.transpose() << endl;
#endif

        // do the SVD thang
        JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
        Matrix3d V = svd.matrixV();
        Matrix3d R = V * svd.matrixU().transpose();          
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
          {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
          }
        Vector3d tr = c0-R*c1;    // translation 

        //        cout << "[PE test] R: " << endl << R << endl;
        //        cout << "[PE test] t: " << tr.transpose() << endl;

#if 0
        R << 0.9997, 0.002515, 0.02418,
          -0.002474, .9999, -0.001698,
          -0.02418, 0.001638, 0.9997;
        tr << -0.005697, -0.01753, 0.05666;
        R = R.transpose();
        tr = -R*tr;
#endif

        // transformation matrix, 3x4
        Matrix<double,3,4> tfm;
        //        tfm.block<3,3>(0,0) = R.transpose();
        //        tfm.col(3) = -R.transpose()*tr;
        tfm.block<3,3>(0,0) = R;
        tfm.col(3) = tr;
        
        //        cout << "[PE test] T: " << endl << tfm << endl;

        // find inliers, based on image reprojection
        int inl = 0;
        for (int i=0; i<nmatch; i++)
          {
            Vector3d pt = tfm*f1.pts[m1[i]];
            Vector3d ipt = f0.cam2pix(pt);
            const cv::KeyPoint &kp = f0.kpts[m0[i]];
            double dx = kp.pt.x - ipt.x();
            double dy = kp.pt.y - ipt.y();
            double dd = f0.disps[m0[i]] - ipt.z();
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 && 
                dd*dd < maxInlierDDist2)
              inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
              //              inl++;
          }
        
        #pragma omp critical
        if (inl > bestinl) 
          {
            bestinl = inl;
            rot = R;
            trans = tr;
          }
      }

    //    printf("Best inliers: %d\n", bestinl);
    //    printf("Total ransac: %d  Neg det: %d\n", ntot, nneg);

    // reduce matches to inliers
    vector<cv::DMatch> inls;    // temporary for current inliers
    inliers.clear();
    Matrix<double,3,4> tfm;
    tfm.block<3,3>(0,0) = rot;
    tfm.col(3) = trans;

    nmatch = matches.size();
    for (int i=0; i<nmatch; i++)
      {
        if (!f0.goodPts[matches[i].queryIdx] || !f1.goodPts[matches[i].trainIdx])
          continue;
        Vector3d pt = tfm*f1.pts[matches[i].trainIdx];
        Vector3d ipt = f0.cam2pix(pt);
        const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
        double dx = kp.pt.x - ipt.x();
        double dy = kp.pt.y - ipt.y();
        double dd = f0.disps[matches[i].queryIdx] - ipt.z();
        if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 && 
            dd*dd < maxInlierDDist2)
          inls.push_back(matches[i]);
      }

#if 0
    cout << endl << trans.transpose().head(3) << endl << endl;
    cout << rot << endl;
#endif

    // polish with stereo sba
    if (polish)
      {
        // system
        SysSBA sba;
        sba.verbose = 0;

        // set up nodes
        // should have a frame => node function        
        Vector4d v0 = Vector4d(0,0,0,1);
        Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
        sba.addNode(v0, q0, f0.cam, true);
        
        Quaterniond qr1(rot);   // from rotation matrix
        Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);

        //        sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
        qr1.normalize();
        sba.addNode(temptrans, qr1, f1.cam, false);

        int in = 3;
        if (in > (int)inls.size())
          in = inls.size();

        // set up projections
        for (int i=0; i<(int)inls.size(); i++)
          {
            // add point
            int i0 = inls[i].queryIdx;
            int i1 = inls[i].trainIdx;
            Vector4d pt = f0.pts[i0];
            sba.addPoint(pt);
            
            // projected point, ul,vl,ur
            Vector3d ipt;
            ipt(0) = f0.kpts[i0].pt.x;
            ipt(1) = f0.kpts[i0].pt.y;
            ipt(2) = ipt(0)-f0.disps[i0];
            sba.addStereoProj(0, i, ipt);

            // projected point, ul,vl,ur
            ipt(0) = f1.kpts[i1].pt.x;
            ipt(1) = f1.kpts[i1].pt.y;
            ipt(2) = ipt(0)-f1.disps[i1];
            sba.addStereoProj(1, i, ipt);
          }

        sba.huber = 2.0;
        sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
        int nbad = sba.removeBad(2.0);
//        cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
        sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);

//        cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;

        // get the updated transform
	      trans = sba.nodes[1].trans.head(3);
	      Quaterniond q1;
	      q1 = sba.nodes[1].qrot;
	      rot = q1.toRotationMatrix();

        // set up inliers
        inliers.clear();
        for (int i=0; i<(int)inls.size(); i++)
          {
            ProjMap &prjs = sba.tracks[i].projections;
            if (prjs[0].isValid && prjs[1].isValid) // valid track
              inliers.push_back(inls[i]);
          }
#if 0
        printf("Inliers: %d   After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
      }

    return (int)inliers.size();
  }
示例#8
0
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1,
        const std::vector<cv::DMatch> &matches)
{
    // convert keypoints in match to 3d points
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p0; // homogeneous coordinates
    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p1;

    int nmatch = matches.size();
    //srand(getDoubleTime());

    // set up data structures for fast processing
    // indices to good matches
    std::vector<int> m0, m1;
    for (int i=0; i<nmatch; i++)
    {
        m0.push_back(matches[i].queryIdx);
        m1.push_back(matches[i].trainIdx);
        //std::cout<<m0[i]<<" "<<m1[i]<<std::endl;
    }

    nmatch = m0.size();

    if (nmatch < 3) return 0;   // can't do it...

    int bestinl = 0;

    // RANSAC loop
//#pragma omp parallel for shared( bestinl )
    for (int i=0; i<numRansac; i++)
    {
        //std::cout << "ransac loop : " << i << std::endl;
        // find a candidate
        int a=rand()%nmatch;
        int b = a;
        while (a==b)
            b=rand()%nmatch;
        int c = a;
        while (a==c || b==c)
            c=rand()%nmatch;

        int i0a = m0[a];
        int i0b = m0[b];
        int i0c = m0[c];
        int i1a = m1[a];
        int i1b = m1[b];
        int i1c = m1[c];

        if (i0a == i0b || i0a == i0c || i0b == i0c ||
                i1a == i1b || i1a == i1c || i1b == i1c)
            continue;

        //std::cout<<a<<" "<<b<<" "<<c<<std::endl;
        //std::cout<<i0a<<" "<<i0b<<" "<<i0c<<std::endl;
        //std::cout<<i1a<<" "<<i1b<<" "<<i1c<<std::endl;

        // get centroids
        Eigen::Vector3d p0a = f0.pts[i0a].head(3);
        Eigen::Vector3d p0b = f0.pts[i0b].head(3);
        Eigen::Vector3d p0c = f0.pts[i0c].head(3);
        Eigen::Vector3d p1a = f1.pts[i1a].head(3);
        Eigen::Vector3d p1b = f1.pts[i1b].head(3);
        Eigen::Vector3d p1c = f1.pts[i1c].head(3);

        Eigen::Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
        Eigen::Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);

        //std::cout<<c0.transpose()<<std::endl;
        //std::cout<<c1.transpose()<<std::endl;
        // subtract out
        p0a -= c0;
        p0b -= c0;
        p0c -= c0;
        p1a -= c1;
        p1b -= c1;
        p1c -= c1;

        Eigen::Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
                            p1c*p0c.transpose();

        // do the SVD thang
        Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix3d V = svd.matrixV();
        Eigen::Matrix3d R = V * svd.matrixU().transpose();
        double det = R.determinant();
        //ntot++;
        if (det < 0.0)
        {
            //nneg++;
            V.col(2) = V.col(2)*-1.0;
            R = V * svd.matrixU().transpose();
        }
        Eigen::Vector3d tr = c0-R*c1;    // translation

        // transformation matrix, 3x4
        Eigen::Matrix<double,3,4> tfm;
        //        tfm.block<3,3>(0,0) = R.transpose();
        //        tfm.col(3) = -R.transpose()*tr;
        tfm.block<3,3>(0,0) = R;
        tfm.col(3) = tr;

#if 0
        // find inliers, based on image reprojection
        int inl = 0;
        for (int i=0; i<nmatch; i++)
        {
            Vector3d pt = tfm*f1.pts[m1[i]];
            Vector3d ipt = f0.cam2pix(pt);
            const cv::KeyPoint &kp = f0.kpts[m0[i]];
            double dx = kp.pt.x - ipt.x();
            double dy = kp.pt.y - ipt.y();
            double dd = f0.disps[m0[i]] - ipt.z();
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                    dd*dd < maxInlierDDist2)
            {
                inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
//		 inl+=(int)sqrt(ipt.z()/matches[i].distance);
//		 cout << "matches[i].distance : " << matches[i].distance << endl;
//		 inl++;
            }
        }
#endif
        int inl = 0;
        for (int i=0; i<nmatch; i++)
        {
            Eigen::Vector3d pt1 = tfm*f1.pts[m1[i]];
            Eigen::Vector3d pt0 = f0.pts[m0[i]].head(3);

//	       double z = fabs(pt1.z() - pt0.z())*0.5;
            double z = pt1.z();
            double dx = pt1.x() - pt0.x();
            double dy = pt1.y() - pt0.y();
            double dd = pt1.z() - pt0.z();

            if (projectMatches)
            {
                // The central idea here is to divide by the distance (this is essentially what cam2pix does).
                dx = dx / z;
                dy = dy / z;
            }
            if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                    dd*dd < maxInlierDDist2)
            {
//----		    inl+=(int)sqrt(pt0.z()); // clever way to weight closer points
//		 inl+=(int)sqrt(ipt.z()/matches[i].distance);
//		 cout << "matches[i].distance : " << matches[i].distance << endl;
                inl++;
            }
        }

//#pragma omp critical
        if (inl > bestinl)
        {
            bestinl = inl;
            rot = R;
            trans = tr;
//	       std::cout << "bestinl : " << bestinl << std::endl;
        }
    }

    //printf("Best inliers: %d\n", bestinl);
    //printf("Total ransac: %d  Neg det: %d\n", ntot, nneg);

    // reduce matches to inliers
    std::vector<cv::DMatch> inls;    // temporary for current inliers
    inliers.clear();
    Eigen::Matrix<double,3,4> tfm;
    tfm.block<3,3>(0,0) = rot;
    tfm.col(3) = trans;

    //std::cout<<"f0: "<<f0.pts.size()<<" "<<f0.kpts.size()<<" "<<f0.pc_kpts.size()<<std::endl;
    //std::cout<<"f1: "<<f1.pts.size()<<" "<<f1.kpts.size()<<" "<<f1.pc_kpts.size()<<std::endl;

    nmatch = matches.size();
    for (int i=0; i<nmatch; i++)
    {
        Eigen::Vector3d pt1 = tfm*f1.pts[matches[i].trainIdx];
        //Eigen::Vector3d pt1_unchanged = f1.pts[matches[i].trainIdx].head(3);
        //Vector3d pt1 = pt1_unchanged;
#if 0
        Vector3d ipt = f0.cam2pix(pt);
        const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
        double dx = kp.pt.x - ipt.x();
        double dy = kp.pt.y - ipt.y();
        double dd = f0.disps[matches[i].queryIdx] - ipt.z();
#endif
        Eigen::Vector3d pt0 = f0.pts[matches[i].queryIdx].head(3);

        //double z = fabs(pt1.z() - pt0.z())*0.5;
        double z = pt1.z();
        double dx = pt1.x() - pt0.x();
        double dy = pt1.y() - pt0.y();
        double dd = pt1.z() - pt0.z();

        if (projectMatches)
        {
            // The central idea here is to divide by the distance (this is essentially what cam2pix does).
            dx = dx / z;
            dy = dy / z;
        }

        if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
                dd*dd < maxInlierDDist2)
        {
            if (z < maxDist && z > minDist)

//	       if (fabs(f0.kpts[matches[i].queryIdx].pt.y - f1.kpts[matches[i].trainIdx].pt.y) > 300)
            {
//		   std::cout << " ---------- " << dx << "," << dy << "," << dd << ",\npt0 " << pt0.transpose() << "\npt1 " << pt1.transpose() << f0.kpts[matches[i].queryIdx].pt << "," <<
//			 f1.kpts[matches[i].trainIdx].pt << "\n unchanged pt1 " << pt1_unchanged.transpose() << std::endl;
                inliers.push_back(matches[i]);
            }
        }
    }

#if 0
    // Test with the SBA...
    {
        // system
        SysSBA sba;
        sba.verbose = 0;

#if 0
        // set up nodes
        // should have a frame => node function
        Vector4d v0 = Vector4d(0,0,0,1);
        Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
        sba.addNode(v0, q0, f0.cam, true);

        Quaterniond qr1(rot);   // from rotation matrix
        Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);

        //        sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
        qr1.normalize();
        sba.addNode(temptrans, qr1, f1.cam, false);

        int in = 3;
        if (in > (int)inls.size())
            in = inls.size();

        // set up projections
        for (int i=0; i<(int)inls.size(); i++)
        {
            // add point
            int i0 = inls[i].queryIdx;
            int i1 = inls[i].trainIdx;
            Vector4d pt = f0.pts[i0];
            sba.addPoint(pt);

            // projected point, ul,vl,ur
            Vector3d ipt;
            ipt(0) = f0.kpts[i0].pt.x;
            ipt(1) = f0.kpts[i0].pt.y;
            ipt(2) = ipt(0)-f0.disps[i0];
            sba.addStereoProj(0, i, ipt);

            // projected point, ul,vl,ur
            ipt(0) = f1.kpts[i1].pt.x;
            ipt(1) = f1.kpts[i1].pt.y;
            ipt(2) = ipt(0)-f1.disps[i1];
            sba.addStereoProj(1, i, ipt);
        }

        sba.huber = 2.0;
        sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
        int nbad = sba.removeBad(2.0); // 2.0
        cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
        sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);

//        cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;

        // get the updated transform
        trans = sba.nodes[1].trans.head(3);
        Quaterniond q1;
        q1 = sba.nodes[1].qrot;
        quat = q1;
        rot = q1.toRotationMatrix();

        // set up inliers
        inliers.clear();
        for (int i=0; i<(int)inls.size(); i++)
        {
            ProjMap &prjs = sba.tracks[i].projections;
            if (prjs[0].isValid && prjs[1].isValid) // valid track
                inliers.push_back(inls[i]);
        }

        printf("Inliers: %d   After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
    }
#endif

//     std::cout << std::endl << trans.transpose().head(3) << std::endl << std::endl;
//     std::cout << rot << std::endl;

    return inliers.size();
}