Note: This tutorial assumes that you have completed the previous tutorials: Performing SBA on Data from a File.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Setting up an SBA System

Description: How to create an SBA system from an external source of data, perform SBA, and then visualize the results.

Tutorial Level: INTERMEDIATE

This tutorial will cover creating an SBA system from an arbitrary source of data. In this case, we will create a synthetic data set to perform SBA on. This assumes you've done the previous tutorial, so if not, follow the instructions for creating the learning_sba package.

Synthetic System

For this tutorial, we will create a synthetic system, shown below.

The system consists of points uniformly spaced on a plane (points shown as red Xs), and 5 camera poses translated horizontally, parallel to the plane (cameras are shown as numbered blue triangles).

cameras.png

After setting up the system and calculating the projections of the points into image coordinates, we will introduce error to the node position and point position estimates.

Parts of an SBA System

Nodes

Nodes, or camera poses, have several attributes:

  • Translation (from the world coordinate frame to the node position)
  • Rotation (from the world coordinate frame to the node position)
  • Camera projection matrix

Points

Points have only one parameter:

  • Estimated position (in x, y, z, w, though w is always 1.0 in the current implementation).

Projections

Projections are constraints between nodes and points, and represent the projection of a point in the image plane of the node. They have several parameters:

  • Node index
  • Point index
  • Projection onto the image plane (in u, v for monocular projections and u, v, d for stereo projections).

The Code

Create a file called src/sba_system_setup.cpp and paste the following code in.

   1 #include <ros/ros.h>
   2 #include <sba/sba.h>
   3 #include <sba/visualization.h>
   4 
   5 
   6 // For random seed.
   7 #include <time.h>
   8 
   9 #include <visualization_msgs/Marker.h>
  10 
  11 using namespace sba;
  12 using namespace std;
  13 
  14 void setupSBA(SysSBA &sys)
  15 {
  16     // Create camera parameters.
  17     frame_common::CamParams cam_params;
  18     cam_params.fx = 430; // Focal length in x
  19     cam_params.fy = 430; // Focal length in y
  20     cam_params.cx = 320; // X position of principal point
  21     cam_params.cy = 240; // Y position of principal point
  22     cam_params.tx = 0;   // Baseline (no baseline since this is monocular)
  23 
  24     // Define dimensions of the image.
  25     int maxx = 640;
  26     int maxy = 480;
  27 
  28     // Create a plane containing a wall of points.
  29     int npts_x = 10; // Number of points on the plane in x
  30     int npts_y = 5;  // Number of points on the plane in y
  31     
  32     double plane_width = 5;     // Width of the plane on which points are positioned (x)
  33     double plane_height = 2.5;    // Height of the plane on which points are positioned (y)
  34     double plane_distance = 5; // Distance of the plane from the cameras (z)
  35 
  36     // Vector containing the true point positions.
  37     vector<Point, Eigen::aligned_allocator<Point> > points;
  38 
  39     for (int ix = 0; ix < npts_x ; ix++)
  40     {
  41       for (int iy = 0; iy < npts_y ; iy++)
  42       {
  43         // Create a point on the plane in a grid.
  44         points.push_back(Point(plane_width/npts_x*(ix+.5), -plane_height/npts_y*(iy+.5), plane_distance, 1.0));
  45       }
  46     }
  47     
  48     // Create nodes and add them to the system.
  49     unsigned int nnodes = 5; // Number of nodes.
  50     double path_length = 3; // Length of the path the nodes traverse.
  51     
  52     unsigned int i = 0, j = 0;
  53     
  54     for (i = 0; i < nnodes; i++)
  55     { 
  56       // Translate in the x direction over the node path.
  57       Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
  58             
  59       // Don't rotate.
  60       Quaterniond rot(1, 0, 0, 0);
  61       rot.normalize();
  62       
  63       // Add a new node to the system.
  64       sys.addNode(trans, rot, cam_params, false);
  65     }
  66         
  67     // Set the random seed.
  68     unsigned short seed = (unsigned short)time(NULL);
  69     seed48(&seed);
  70         
  71     // Add points into the system, and add noise.
  72     for (i = 0; i < points.size(); i++)
  73     {
  74       // Add up to .5 points of noise.
  75       Vector4d temppoint = points[i];
  76       temppoint.x() += drand48() - 0.5;
  77       temppoint.y() += drand48() - 0.5;
  78       temppoint.z() += drand48() - 0.5;
  79       sys.addPoint(temppoint);
  80     }
  81     
  82     Vector2d proj;
  83     
  84     // Project points into nodes.
  85     for (i = 0; i < points.size(); i++)
  86     {
  87       for (j = 0; j < sys.nodes.size(); j++)
  88       {
  89         // Project the point into the node's image coordinate system.
  90         sys.nodes[j].setProjection();
  91         sys.nodes[j].project2im(proj, points[i]);
  92         
  93         // If valid (within the range of the image size), add the monocular 
  94         // projection to SBA.
  95         if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
  96         {
  97           sys.addMonoProj(j, i, proj);
  98         }
  99       }
 100     }
 101     
 102     // Add noise to node position.
 103     
 104     double transscale = 1.0;
 105     double rotscale = 0.2;
 106     
 107     // Don't actually add noise to the first node, since it's fixed.
 108     for (i = 1; i < sys.nodes.size(); i++)
 109     {
 110       Vector4d temptrans = sys.nodes[i].trans;
 111       Quaterniond tempqrot = sys.nodes[i].qrot;
 112       
 113       // Add error to both translation and rotation.
 114       temptrans.x() += transscale*(drand48() - 0.5);
 115       temptrans.y() += transscale*(drand48() - 0.5);
 116       temptrans.z() += transscale*(drand48() - 0.5);
 117       tempqrot.x() += rotscale*(drand48() - 0.5);
 118       tempqrot.y() += rotscale*(drand48() - 0.5);
 119       tempqrot.z() += rotscale*(drand48() - 0.5);
 120       tempqrot.normalize();
 121       
 122       sys.nodes[i].trans = temptrans;
 123       sys.nodes[i].qrot = tempqrot;
 124       
 125       // These methods should be called to update the node.
 126       sys.nodes[i].normRot();
 127       sys.nodes[i].setTransform();
 128       sys.nodes[i].setProjection();
 129       sys.nodes[i].setDr(true);
 130     }
 131         
 132 }
 133 
 134 void processSBA(ros::NodeHandle node)
 135 {
 136     // Create publisher topics.
 137     ros::Publisher cam_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
 138     ros::Publisher point_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/points", 1);
 139     ros::spinOnce();
 140     
 141     ROS_INFO("Sleeping for 2 seconds to publish topics...");
 142     ros::Duration(2.0).sleep();
 143     
 144     // Create an empty SBA system.
 145     SysSBA sys;
 146     
 147     setupSBA(sys);
 148     
 149     // Provide some information about the data read in.
 150     ROS_INFO("Cameras (nodes): %d, Points: %d",
 151         (int)sys.nodes.size(), (int)sys.tracks.size());
 152         
 153     // Publish markers
 154     drawGraph(sys, cam_marker_pub, point_marker_pub);
 155     ros::spinOnce();
 156     
 157     ROS_INFO("Sleeping for 5 seconds to publish pre-SBA markers.");
 158     ros::Duration(5.0).sleep();
 159         
 160     // Perform SBA with 10 iterations, an initial lambda step-size of 1e-3, 
 161     // and using CSPARSE.
 162     sys.doSBA(10, 1e-3, SBA_SPARSE_CHOLESKY);
 163     
 164     int npts = sys.tracks.size();
 165 
 166     ROS_INFO("Bad projs (> 10 pix): %d, Cost without: %f", 
 167         (int)sys.countBad(10.0), sqrt(sys.calcCost(10.0)/npts));
 168     ROS_INFO("Bad projs (> 5 pix): %d, Cost without: %f", 
 169         (int)sys.countBad(5.0), sqrt(sys.calcCost(5.0)/npts));
 170     ROS_INFO("Bad projs (> 2 pix): %d, Cost without: %f", 
 171         (int)sys.countBad(2.0), sqrt(sys.calcCost(2.0)/npts));
 172     
 173     ROS_INFO("Cameras (nodes): %d, Points: %d",
 174         (int)sys.nodes.size(), (int)sys.tracks.size());
 175         
 176     // Publish markers
 177     drawGraph(sys, cam_marker_pub, point_marker_pub);
 178     ros::spinOnce();
 179     ROS_INFO("Sleeping for 5 seconds to publish post-SBA markers.");
 180     ros::Duration(5.0).sleep();
 181 }
 182 
 183 int main(int argc, char **argv)
 184 {
 185     ros::init(argc, argv, "sba_system_setup");
 186     
 187     ros::NodeHandle node;
 188     
 189     processSBA(node);
 190     ros::spinOnce();
 191 
 192     return 0;
 193 }

The Code Explained

  17     frame_common::CamParams cam_params;
  18     cam_params.fx = 430; // Focal length in x
  19     cam_params.fy = 430; // Focal length in y
  20     cam_params.cx = 320; // X position of principal point
  21     cam_params.cy = 240; // Y position of principal point
  22     cam_params.tx = 0;   // Baseline (no baseline since this is monocular)
  23 

Here we set up the camera parameters. These can be set from the CameraInfo topic in ROS, or from the projection matrices from a different camera. For more information on what these values are, see CameraInfo and Camera Calibration and 3D Reconstruction in OpenCV.

  37     vector<Point, Eigen::aligned_allocator<Point> > points;

Points are an Eigen::Vector4d, in the x, y, z, w order, with w having a value of 1.0. We create a local vector of points to contain the true point position.

  57       Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
  58             
  59       // Don't rotate.
  60       Quaterniond rot(1, 0, 0, 0);
  61       rot.normalize();

The translation for a node is an Eigen::Vector4d, in x, y, z, w order. Again, w is always 1.0. The translation is from the origin of the world frame to the position of the node. Rotation of a node is expressed as a Quaterniond expressing the rotation from the origin of the world frame to the node position. For more information about Quaternions, see the Eigen Documentation and the Wikipedia page on them.

We then add the node (at its true position) to the system, as an unfixed node. For more information about the parameters to addNode(), see the SysSBA documentation.

  75       Vector4d temppoint = points[i];
  76       temppoint.x() += drand48() - 0.5;
  77       temppoint.y() += drand48() - 0.5;
  78       temppoint.z() += drand48() - 0.5;
  79       sys.addPoint(temppoint);

Add noise to the estimate of the point positions and then add them to the system. drand48() generates a random number evenly distributed between 0.0 and 1.0, and in this case we shift it to a number between -0.5 and 0.5. Note that this is a substantial amount of error.

  90         sys.nodes[j].setProjection();
  91         sys.nodes[j].project2im(proj, points[i]);

Set up the projection matrix from world coordinates to image coordinates, and then project a point onto the node's image frame.

  95         if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
  96         {
  97           sys.addMonoProj(j, i, proj);
  98         }

If a point's projection is within the boundaries of the camera image, then add the projection linking the node and point to the system.

There are three methods to add projections: addMonoProj(), which adds a monocular projection, addStereoProj(), which adds a stereo projection, and addProj() which adds either kind of projection. In most cases, it is most convenient to use the method for the specific kind of projection you are adding, though addProj() is the most convenient when copying projections that already exist in the SBA system.

 122       sys.nodes[i].trans = temptrans;
 123       sys.nodes[i].qrot = tempqrot;
 124       
 125       // These methods should be called to update the node.
 126       sys.nodes[i].normRot();
 127       sys.nodes[i].setTransform();
 128       sys.nodes[i].setProjection();
 129       sys.nodes[i].setDr(true);

We then add noise to the estimates of the camera pose. Note that you should almost always use addNode() instead of manually manipulating a node, as there are several functions that need to be called to update the internal state of the node (normRot(), setTransform(), setProjection(), and setDr()).

Running

Add the following lines to CMakeLists.txt.

rosbuild_add_executable(sba_system_setup src/sba_system_setup.cpp)
target_link_libraries(sba_system_setup sba_ros)

Make the program, start up rviz (the setup instructions are in the previous tutorial), and then run the program.

 $ rosmake learning_sba
 $ rosrun rviz rviz
 $ rosrun learning_sba sba_system_setup

It should show the pre-SBA markers for 5 seconds, which show a considerable amount of error in the estimates of camera poses and point positions, as shown below.

presba.png

After running SBA, the code will publish post-SBA markers, which should look as below: a plane of points with 5 equally spaced camera poses in front.

postsba.png

Wiki: sba/Tutorials/SettingUpAnSBASystem (last edited 2010-07-14 21:31:16 by HelenOleynikova)