void SBANode::addProj(const sba::Projection& msg)
{
  int camindex = node_indices[msg.camindex];
  int pointindex = point_indices[msg.pointindex];
  Vector3d keypoint(msg.u, msg.v, msg.d);
  bool stereo = msg.stereo;
  bool usecovariance = msg.usecovariance;
  Eigen::Matrix3d covariance;
  covariance << msg.covariance[0], msg.covariance[1], msg.covariance[2],
                msg.covariance[3], msg.covariance[4], msg.covariance[5],
                msg.covariance[6], msg.covariance[7], msg.covariance[8];
  
  // Make sure it's valid before adding it.
  if (pointindex < (int)sba.tracks.size() && camindex < (int)sba.nodes.size())
  {
    sba.addProj(camindex, pointindex, keypoint, stereo);
    if (usecovariance)
      sba.setProjCovariance(camindex, pointindex, covariance);
  }
  else
  {
    ROS_INFO("Failed to add projection: C: %d, P: %d, Csize: %d, Psize: %d", 
            camindex, pointindex,(int)sba.nodes.size(),(int)sba.tracks.size());       
  }
}
void setupSBA(SysSBA &sys)
{
    // Create camera parameters.
    frame_common::CamParams cam_params;
    cam_params.fx = 430; // Focal length in x
    cam_params.fy = 430; // Focal length in y
    cam_params.cx = 320; // X position of principal point
    cam_params.cy = 240; // Y position of principal point
    cam_params.tx = 0.3; // Baseline 

    // Define dimensions of the image.
    int maxx = 640;
    int maxy = 480;

    // Create a plane containing a wall of points.
    Plane middleplane;
    middleplane.resize(3, 2, 10, 5);
    //middleplane.rotate(PI/4.0, PI/6.0, 1, 0);
    middleplane.rotate(PI/4.0, 1, 0, 1);
    middleplane.translate(0.0, 0.0, 5.0);
    
    // Create another plane containing a wall of points.
    Plane mp2;
    mp2.resize(3, 2, 10, 5);
    mp2.rotate(0, 0, 0, 1);
    mp2.translate(0.0, 0.0, 4.0);


    // Create another plane containing a wall of points.
    Plane mp3;
    mp3.resize(3, 2, 10, 5);
    mp3.rotate(-PI/4.0, 1, 0, 1);
    mp3.translate(0.0, 0.0, 4.5);



    // Vector containing the true point positions.
    vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > normals;
    
    points.insert(points.end(), middleplane.points.begin(), middleplane.points.end());
    normals.insert(normals.end(), middleplane.points.size(), middleplane.normal);
    points.insert(points.end(), mp2.points.begin(), mp2.points.end());
    normals.insert(normals.end(), mp2.points.size(), mp2.normal);
    points.insert(points.end(), mp3.points.begin(), mp3.points.end());
    normals.insert(normals.end(), mp3.points.size(), mp3.normal);
    
    // Create nodes and add them to the system.
    unsigned int nnodes = 2; // Number of nodes.
    double path_length = 1.0; // Length of the path the nodes traverse.

    // Set the random seed.
    unsigned short seed = (unsigned short)time(NULL);
    seed48(&seed);
    
    unsigned int i = 0;
    
    Vector3d inormal0 = middleplane.normal;
    Vector3d inormal1 = middleplane.normal;
    Vector3d inormal20 = mp2.normal;
    Vector3d inormal21 = mp2.normal;
    Vector3d inormal30 = mp3.normal;
    Vector3d inormal31 = mp3.normal;
    
    for (i = 0; i < nnodes; i++)
    { 
      // Translate in the x direction over the node path.
      Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
            
#if 1
      if (i >= 2)
	    {
	      // perturb a little
	      double tnoise = 0.5;	// meters
	      trans.x() += tnoise*(drand48()-0.5);
	      trans.y() += tnoise*(drand48()-0.5);
	      trans.z() += tnoise*(drand48()-0.5);
	    }
#endif

      // Don't rotate.
      Quaterniond rot(1, 0, 0, 0);
#if 1
      if (i >= 2)
	    {
	      // perturb a little
	      double qnoise = 0.1;	// meters
	      rot.x() += qnoise*(drand48()-0.5);
	      rot.y() += qnoise*(drand48()-0.5);
	      rot.z() += qnoise*(drand48()-0.5);
	    }
#endif
      rot.normalize();
      
      // Add a new node to the system.
      sys.addNode(trans, rot, cam_params, false);
      
      // set normal
      if (i == 0)
	{
	  inormal0 = rot.toRotationMatrix().transpose() * inormal0;
	  inormal20 = rot.toRotationMatrix().transpose() * inormal20;
	  inormal30 = rot.toRotationMatrix().transpose() * inormal30;
	}
      else
	{
	  inormal1 = rot.toRotationMatrix().transpose() * inormal1;
	  inormal21 = rot.toRotationMatrix().transpose() * inormal21;
	  inormal31 = rot.toRotationMatrix().transpose() * inormal31;
	}
    }
        
    double pointnoise = 1.0;
    
    // Add points into the system, and add noise.
    for (i = 0; i < points.size(); i++)
    {
      // Add up to .5 points of noise.
      Vector4d temppoint = points[i];
      temppoint.x() += pointnoise*(drand48() - 0.5);
      temppoint.y() += pointnoise*(drand48() - 0.5);
      temppoint.z() += pointnoise*(drand48() - 0.5);
      sys.addPoint(temppoint);
    }
    
    // Each point gets added twice
    for (i = 0; i < points.size(); i++)
    {
      // Add up to .5 points of noise.
      Vector4d temppoint = points[i];
      temppoint.x() += pointnoise*(drand48() - 0.5);
      temppoint.y() += pointnoise*(drand48() - 0.5);
      temppoint.z() += pointnoise*(drand48() - 0.5);
      sys.addPoint(temppoint);
    }

    Vector2d proj2d, proj2dp;
    Vector3d proj, projp, pc, pcp, baseline, baselinep;
    
    Vector3d imagenormal(0, 0, 1);
    
    Matrix3d covar0;
    covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2));
    Matrix3d covar;
    
    Quaterniond rotation;
    Matrix3d rotmat;
    
    unsigned int midindex = middleplane.points.size();
    printf("Normal for Middle Plane: [%f %f %f], index %d -> %d\n", middleplane.normal.x(), middleplane.normal.y(), middleplane.normal.z(), 0, midindex-1);
    
    int nn = points.size();

    // Project points into nodes.
    for (i = 0; i < points.size(); i++)
    {
      int k=i;
      // Project the point into the node's image coordinate system.
      sys.nodes[0].setProjection();
      sys.nodes[0].project2im(proj2d, points[k]);
      sys.nodes[1].setProjection();
      sys.nodes[1].project2im(proj2dp, points[k]);
        
      // Camera coords for right camera
      baseline << sys.nodes[0].baseline, 0, 0;
      pc = sys.nodes[0].Kcam * (sys.nodes[0].w2n*points[k] - baseline); 
      proj.head<2>() = proj2d;
      proj(2) = pc(0)/pc(2);
        
      baseline << sys.nodes[1].baseline, 0, 0;
      pcp = sys.nodes[1].Kcam * (sys.nodes[1].w2n*points[k] - baseline); 
      projp.head<2>() = proj2dp;
      projp(2) = pcp(0)/pcp(2);

      // add noise to projections
      double prnoise = 0.5;	// 0.5 pixels
      proj.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5));
      projp.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5));


      // If valid (within the range of the image size), add the stereo 
      // projection to SBA.
      if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
        {
          // add point cloud shape-holding projections to each node
          sys.addStereoProj(0, k, proj);
          sys.addStereoProj(1, k+nn, projp);

#ifdef USE_PP
          // add exact matches
          if (i == 20 || i == 65 || i == 142)
            sys.addStereoProj(1, k, projp);
#endif

#ifdef USE_PPL
          // add point-plane matches
	  if (i < midindex)
            sys.addPointPlaneMatch(0, k, inormal0, 1, k+nn, inormal1);
	  else if (i < 2*midindex)
	    sys.addPointPlaneMatch(0, k, inormal20, 1, k+nn, inormal21);
	  else
	    sys.addPointPlaneMatch(0, k, inormal30, 1, k+nn, inormal31);
          //          sys.addStereoProj(0, k+nn, projp);
          //          sys.addStereoProj(1, k, proj);

          Matrix3d covar;
          double cv = 0.01;
          covar << cv, 0, 0,
            0, cv, 0, 
            0, 0, cv;
          sys.setProjCovariance(0, k+nn, covar);
          sys.setProjCovariance(1, k, covar);
#endif

        }
      else
        {
          cout << "ERROR! point not in view of nodes" << endl;
          //return;
        }
    }

    // Add noise to node position.
    
    double transscale = 2.0;
    double rotscale = 0.5;
    
    // Don't actually add noise to the first node, since it's fixed.
    for (i = 1; i < sys.nodes.size(); i++)
    {
      Vector4d temptrans = sys.nodes[i].trans;
      Quaterniond tempqrot = sys.nodes[i].qrot;
      
      // Add error to both translation and rotation.
      temptrans.x() += transscale*(drand48() - 0.5);
      temptrans.y() += transscale*(drand48() - 0.5);
      temptrans.z() += transscale*(drand48() - 0.5);
      tempqrot.x() += rotscale*(drand48() - 0.5);
      tempqrot.y() += rotscale*(drand48() - 0.5);
      tempqrot.z() += rotscale*(drand48() - 0.5);
      tempqrot.normalize();
      
      sys.nodes[i].trans = temptrans;
      sys.nodes[i].qrot = tempqrot;
      
      // These methods should be called to update the node.
      sys.nodes[i].normRot();
      sys.nodes[i].setTransform();
      sys.nodes[i].setProjection();
      sys.nodes[i].setDr(true);
    }
        
}
void setupSBA(SysSBA &sba)
{
    // Create camera parameters.
    frame_common::CamParams cam_params;
    cam_params.fx = 430; // Focal length in x
    cam_params.fy = 430; // Focal length in y
    cam_params.cx = 320; // X position of principal point
    cam_params.cy = 240; // Y position of principal point
    cam_params.tx = -30; // Baseline (no baseline since this is monocular)

    // Create a plane containing a wall of points.
    Plane plane0, plane1;
    plane0.resize(3, 2, 10, 5);
    
    plane1 = plane0;
    plane1.translate(0.1, 0.05, 0.0);
    
    plane1.rotate(PI/4.0, 1, 0, 0);
    plane1.translate(0.0, 0.0, 7.0);
    
    
    plane0.rotate(PI/4.0, 1, 0, 0);
    plane0.translate(0.0, 0.0, 7.0);
    
    //plane1.translate(0.05, 0.0, 0.05);
    
    // Create nodes and add them to the system.
    unsigned int nnodes = 2; // Number of nodes.
    double path_length = 2; // Length of the path the nodes traverse.

    // Set the random seed.
    unsigned short seed = (unsigned short)time(NULL);
    seed48(&seed);
    
    for (int i = 0; i < nnodes; i++)
    { 
      // Translate in the x direction over the node path.
      Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
            
#if 0
      if (i >= 0)
	    {
	      // perturb a little
	      double tnoise = 0.5;	// meters
	      trans.x() += tnoise*(drand48()-0.5);
	      trans.y() += tnoise*(drand48()-0.5);
	      trans.z() += tnoise*(drand48()-0.5);
	    }
#endif

      // Don't rotate.
      Quaterniond rot(1, 0, 0, 0);
#if 0
      if (i > 0)
	    {
	      // perturb a little
	      double qnoise = 0.1;	// meters
	      rot.x() += qnoise*(drand48()-0.5);
	      rot.y() += qnoise*(drand48()-0.5);
	      rot.z() += qnoise*(drand48()-0.5);
	    }
#endif
      rot.normalize();
      
      // Add a new node to the system.
      sba.addNode(trans, rot, cam_params, false);
    }
    
    Vector3d imagenormal(0, 0, 1);
    
    Matrix3d covar0;
    covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2));
    Matrix3d covar;
    
    Quaterniond rotation;
    Matrix3d rotmat;
    
    // Project points into nodes.
    addPointAndProjection(sba, plane0.points, 0);
    addPointAndProjection(sba, plane1.points, 1);
    
    int offset = plane0.points.size();
    
    Vector3d normal0 = sba.nodes[0].qrot.toRotationMatrix().transpose()*plane0.normal; 
    Vector3d normal1 = sba.nodes[1].qrot.toRotationMatrix().transpose()*plane1.normal; 
    
    printf("Normal: %f %f %f -> %f %f %f\n", plane0.normal.x(), plane0.normal.y(), plane0.normal.z(), normal0.x(), normal0.y(), normal0.z());
    printf("Normal: %f %f %f -> %f %f %f\n", plane1.normal.x(), plane1.normal.y(), plane1.normal.z(), normal1.x(), normal1.y(), normal1.z());
    
    for (int i = 0; i < plane0.points.size(); i++)
    {
      sba.addPointPlaneMatch(0, i, normal0, 1, i+offset, normal1);

      Matrix3d covar;
      covar << 0.1, 0, 0,
                0, 0.1, 0, 
          	0, 0, 0.1;
      sba.setProjCovariance(0, i+offset, covar);
      sba.setProjCovariance(1, i, covar);
    }
    
    // Add noise to node position.
    
    double transscale = 1.0;
    double rotscale = 0.1;
    
    // Don't actually add noise to the first node, since it's fixed.
    for (int i = 1; i < sba.nodes.size(); i++)
    {
      Vector4d temptrans = sba.nodes[i].trans;
      Quaterniond tempqrot = sba.nodes[i].qrot;
      
      // Add error to both translation and rotation.
      temptrans.x() += transscale*(drand48() - 0.5);
      temptrans.y() += transscale*(drand48() - 0.5);
      temptrans.z() += transscale*(drand48() - 0.5);
      tempqrot.x() += rotscale*(drand48() - 0.5);
      tempqrot.y() += rotscale*(drand48() - 0.5);
      tempqrot.z() += rotscale*(drand48() - 0.5);
      tempqrot.normalize();
      
      sba.nodes[i].trans = temptrans;
      sba.nodes[i].qrot = tempqrot;
      
      // These methods should be called to update the node.
      sba.nodes[i].normRot();
      sba.nodes[i].setTransform();
      sba.nodes[i].setProjection();
      sba.nodes[i].setDr(true);
    }
}
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();
      }

}