C++ SysSBA::doSBA方法代码示例

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();
}

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

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

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();
      }

}

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

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