| Note: This tutorial assumes that you have completed the previous tutorials: Introduction to SBA. |
 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. |
Performing SBA on Data from File
Description: This tutorial shows how to perform SBA on data from a file.Tutorial Level: INTERMEDIATE
Next Tutorial: Setting up an SBA System
In this tutorial, we are going to walk through using SBA on point position and camera pose data loaded from a file.
To begin, let's make a new package in ros_pkg_tutorials called learning_sba, which depends on roscpp and sba.
$ roscd ros_pkg_tutorials $ roscreate-pkg learning_sba roscpp sba $ rosmake learning_sba $ roscd learning_sba
Reading Data from File
First, we want to read bundle adjustment data in from a file. For this tutorial, we will use the Bundler format, documented at The Bundler User's Manual. To do the tutorial, you should download a sample bundler file from sample_bundler_file.out. The file features a synthetic situation (discussed in more detail in the following tutorial) with introduced error.
The Code
Create a file called src/sba_from_file.cpp and paste the following code in.
1 #include <ros/ros.h>
2
3 #include <sba/sba.h>
4 #include <sba/sba_file_io.h>
5 #include <sba/visualization.h>
6
7 #include <visualization_msgs/Marker.h>
8
9 using namespace sba;
10 using namespace std;
11
12 void processSBAfile(char* filename, ros::NodeHandle node)
13 {
14 // Create publisher topics.
15 ros::Publisher cam_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
16 ros::Publisher point_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/points", 1);
17 ros::spinOnce();
18
19 ROS_INFO("Sleeping for 2 seconds to publish topics...");
20 ros::Duration(2.0).sleep();
21
22 // Create an empty SBA system.
23 SysSBA sys;
24
25 // Read in information from the bundler file.
26 readBundlerFile(filename, sys);
27
28 // Provide some information about the data read in.
29 ROS_INFO("Cameras (nodes): %d, Points: %d",
30 (int)sys.nodes.size(), (int)sys.tracks.size());
31
32 // Publish markers
33 drawGraph(sys, cam_marker_pub, point_marker_pub);
34 ros::spinOnce();
35
36 ROS_INFO("Sleeping for 5 seconds to publish pre-SBA markers.");
37 ros::Duration(5.0).sleep();
38
39 // Perform SBA with 10 iterations, an initial lambda step-size of 1e-3,
40 // and using CSPARSE.
41 sys.doSBA(10, 1e-3, 1);
42
43 int npts = sys.tracks.size();
44
45 ROS_INFO("Bad projs (> 10 pix): %d, Cost without: %f",
46 (int)sys.countBad(10.0), sqrt(sys.calcCost(10.0)/npts));
47 ROS_INFO("Bad projs (> 5 pix): %d, Cost without: %f",
48 (int)sys.countBad(5.0), sqrt(sys.calcCost(5.0)/npts));
49 ROS_INFO("Bad projs (> 2 pix): %d, Cost without: %f",
50 (int)sys.countBad(2.0), sqrt(sys.calcCost(2.0)/npts));
51
52 ROS_INFO("Cameras (nodes): %d, Points: %d",
53 (int)sys.nodes.size(), (int)sys.tracks.size());
54
55 // Publish markers
56 drawGraph(sys, cam_marker_pub, point_marker_pub);
57 ros::spinOnce();
58 ROS_INFO("Sleeping for 5 seconds to publish post-SBA markers.");
59 ros::Duration(5.0).sleep();
60 }
61
62 int main(int argc, char **argv)
63 {
64 ros::init(argc, argv, "sba_from_file");
65
66 ros::NodeHandle node;
67
68 if (argc < 2)
69 {
70 ROS_ERROR("Arguments are: <input filename>");
71 return -1;
72 }
73 char* filename = argv[1];
74
75 processSBAfile(filename, node);
76 ros::spinOnce();
77
78 return 0;
79 }
The Code Explained
The first thing we do is include the relevant files. Note that it is not necessary to use ROS to use the SBA package; we simply choose to in this tutorial.
Here we include the sba package header file. bundler_file.h provides the methods necessary to read SBA data in from a bundler-formatted file and visualization.h provides methods for visualizing camera pose and point positions in rviz.
26 readBundlerFile(filename, sys);
This is the function that fills in the SysSBA object from a bundler file. The file contains node information (camera poses), point position estimates, and projection information. It may be instructive to look at the code of read_bundler_to_sba(). The next tutorial shows how to set up an SBA system from an arbitrary source of points.
This displays the number of nodes and points in the system. sys.nodes is a std::vector containing all of the nodes of the system, and sys.tracks is a std::vector containing all the world points and their projections. The next tutorial will cover these structures in more depth.
41 sys.doSBA(10, 1e-3, 1);
Perform the actual SBA operation in 10 iterations, with an initial Levenberg–Marquardt lambda of 1.0e-3, and using the CHOLMOD matrix factorization method (indicated by 1 - this will hopefully be a #define in the future).
56 drawGraph(sys, cam_marker_pub, point_marker_pub);
Publish camera markers and point markers to visualization topics to be viewed in rviz.
Running
Add the following lines to CMakeLists.txt.
rosbuild_add_executable(sba_from_file src/sba_from_file.cpp) target_link_libraries(sba_from_file sba_ros)
Then make and run the program with the path of the bundle file as an argument.
$ rosmake learning_sba $ rosrun learning_sba sba_from_file ~/path/to/file.out
Your output should look like:
[ INFO] [1279059974.914391844]: Sleeping for 2 seconds to publish topics... Found Bundler 3.0 file Number of cameras: 5 Number of points: 50 Reading in camera data...done Reading in pts data...done Number of projections: 230 Average projections per camera: 46 Average track length: 4.6 [ INFO] [1279059976.916259450]: Cameras (nodes): 5, Points: 50 [ INFO] [1279059976.916415304]: Sleeping for 5 seconds to publish pre-SBA markers. 0 Initial squared cost: 2.62473e+06 which is 106.826 rms pixel error and 96.2186 average reproj error; 0 bad points 0 Updated squared cost: 30778.5 which is 11.568 rms pixel error and 8.81095 average reproj error; 0 bad points [SBA] Setup: 0.23 ms Solve: 0.06 ms Update: 0.06 ms 1 Updated squared cost: 373.224 which is 1.27386 rms pixel error and 1.02115 average reproj error; 0 bad points 2 Updated squared cost: 7.60405 which is 0.181827 rms pixel error and 0.148354 average reproj error; 0 bad points 3 Updated squared cost: 0.253204 which is 0.0331796 rms pixel error and 0.0296173 average reproj error; 0 bad points 4 Updated squared cost: 0.0342328 which is 0.0121999 rms pixel error and 0.0106206 average reproj error; 0 bad points 5 Updated squared cost: 0.00207273 which is 0.00300198 rms pixel error and 0.00259849 average reproj error; 0 bad points 6 Updated squared cost: 4.05493e-05 which is 0.000419883 rms pixel error and 0.000361946 average reproj error; 0 bad points 7 Updated squared cost: 2.32171e-07 which is 3.17716e-05 rms pixel error and 2.7287e-05 average reproj error; 0 bad points 8 Updated squared cost: 3.61734e-10 which is 1.2541e-06 rms pixel error and 1.07545e-06 average reproj error; 0 bad points 9 Updated squared cost: 1.46772e-13 which is 2.52614e-08 rms pixel error and 2.16564e-08 average reproj error; 0 bad points [ INFO] [1279059981.919325923]: Bad projs (> 10 pix): 0, Cost without: 0.000000 [ INFO] [1279059981.919433751]: Bad projs (> 5 pix): 0, Cost without: 0.000000 [ INFO] [1279059981.919481971]: Bad projs (> 2 pix): 0, Cost without: 0.000000 [ INFO] [1279059981.919514588]: Cameras (nodes): 5, Points: 50 [ INFO] [1279059981.919592690]: Sleeping for 5 seconds to publish post-SBA markers.
What does this mean? Though the the system started with a substantial amount of error (106 RMS error), after only 2 iterations, the RMS error is down to the sub-pixel range. You can see the substantial difference between the original system and the optimized system by visualizing the data with rviz, as explained below.
Visualization with Rviz
The easiest way to look at this data in rviz is to run rviz and load this rviz configuration file. If you'd rather do it by hand, simply change the fixed frame to pgraph and add the sba/points and sba/cameras marker topics. Run rviz prior to executing sba_from_file so that it will catch the published data before it expires.
The results should look something like this:
The teal shows camera pose, and red shows the point positions.
The next tutorial will show you how to set up an SBA system from an arbitrary source of data. In the case of the tutorial, it will be a synthetic system. Click to see the Setting up an SBA System tutorial.
