| 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
Contents
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).
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.
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.
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.
Set up the projection matrix from world coordinates to image coordinates, and then project a point onto the node's image frame.
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.
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.
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.
