sba_node.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <ros/time.h>
3 
4 // Messages
5 #include <sparse_bundle_adjustment/Frame.h>
6 #include <visualization_msgs/Marker.h>
8 
11 
12 #include <map>
13 
14 #include <ros/callback_queue.h>
15 
16 
17 using namespace sba;
18 
19 class SBANode
20 {
21  public:
23  ros::NodeHandle n;
24  ros::Subscriber frame_sub;
25  ros::Publisher cam_marker_pub;
26  ros::Publisher point_marker_pub;
27 
28  ros::Timer timer_sba;
29  ros::Timer timer_vis;
30 
31  // Mapping from external point index to internal (sba) point index
32  std::map<unsigned int, unsigned int> point_indices;
33 
34  // Mapping from external node index to internal (sba) node index
35  std::map<unsigned int, unsigned int> node_indices;
36 
37  void addFrame(const sba::Frame::ConstPtr& msg);
38  void addNode(const sba::CameraNode& msg);
39  void addPoint(const sba::WorldPoint& msg);
40  void addProj(const sba::Projection& msg);
41  void doSBA(/*const ros::TimerEvent& event*/);
42  void publishTopics(/*const ros::TimerEvent& event*/);
43  SBANode();
44 };
45 
46 void SBANode::addFrame(const sba::Frame::ConstPtr& msg)
47 {
48  unsigned int i = 0;
49 
50  ros::Time beginning = ros::Time::now();
51 
52  // Add all nodes
53  for (i=0; i < msg->nodes.size(); i++)
54  {
55  addNode(msg->nodes[i]);
56  }
57 
58  // Add all points
59  for (i=0; i < msg->points.size(); i++)
60  {
61  addPoint(msg->points[i]);
62  }
63 
64  // Add all projections
65  for (i=0; i < msg->projections.size(); i++)
66  {
67  addProj(msg->projections[i]);
68  }
69 
70  publishTopics();
71 
72  // Do SBA Every 10 frames.
73  if (sba.nodes.size() % 5 == 0)
74  {
75  doSBA();
76  }
77 
78  ros::Time end = ros::Time::now();
79 
80  printf("[SBA] Added frame with %u nodes, %u points, and %u projections (%f s)\n",
81  (unsigned int)msg->nodes.size(), (unsigned int)msg->points.size(),
82  (unsigned int)msg->projections.size(), (end-beginning).toSec());
83 }
84 
85 void SBANode::addNode(const sba::CameraNode& msg)
86 {
87  Vector4d trans(msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z, 1.0);
88  Quaterniond qrot(msg.transform.rotation.w, msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z);
89 
90  frame_common::CamParams cam_params;
91  cam_params.fx = msg.fx;
92  cam_params.fy = msg.fy;
93  cam_params.cx = msg.cx;
94  cam_params.cy = msg.cy;
95  cam_params.tx = msg.baseline;
96 
97  bool fixed = msg.fixed;
98 
99  unsigned int newindex = sba.addNode(trans, qrot, cam_params, fixed);
100 
101  node_indices[msg.index] = newindex;
102 }
103 
104 void SBANode::addPoint(const sba::WorldPoint& msg)
105 {
106  Vector4d point(msg.x, msg.y, msg.z, msg.w);
107  unsigned int newindex = sba.addPoint(point);
108 
109  point_indices[msg.index] = newindex;
110 }
111 
112 void SBANode::addProj(const sba::Projection& msg)
113 {
114  int camindex = node_indices[msg.camindex];
115  int pointindex = point_indices[msg.pointindex];
116  Vector3d keypoint(msg.u, msg.v, msg.d);
117  bool stereo = msg.stereo;
118  bool usecovariance = msg.usecovariance;
119  Eigen::Matrix3d covariance;
120  covariance << msg.covariance[0], msg.covariance[1], msg.covariance[2],
121  msg.covariance[3], msg.covariance[4], msg.covariance[5],
122  msg.covariance[6], msg.covariance[7], msg.covariance[8];
123 
124  // Make sure it's valid before adding it.
125  if (pointindex < (int)sba.tracks.size() && camindex < (int)sba.nodes.size())
126  {
127  sba.addProj(camindex, pointindex, keypoint, stereo);
128  if (usecovariance)
129  sba.setProjCovariance(camindex, pointindex, covariance);
130  }
131  else
132  {
133  ROS_INFO("Failed to add projection: C: %d, P: %d, Csize: %d, Psize: %d",
134  camindex, pointindex,(int)sba.nodes.size(),(int)sba.tracks.size());
135  }
136 }
137 
138 void SBANode::doSBA(/*const ros::TimerEvent& event*/)
139 {
140  unsigned int projs = 0;
141  // For debugging.
142  for (int i = 0; i < (int)sba.tracks.size(); i++)
143  {
144  projs += sba.tracks[i].projections.size();
145  }
146  ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sba.nodes.size(),
147  (int)sba.tracks.size(), projs);
148 
149  if (sba.nodes.size() > 0)
150  {
151  // Copied from vslam.cpp: refine()
152  sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);
153 
154  double cost = sba.calcRMSCost();
155 
156  if (isnan(cost) || isinf(cost)) // is NaN?
157  {
158  ROS_INFO("NaN cost!");
159  }
160  else
161  {
162  if (sba.calcRMSCost() > 4.0)
163  sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY); // do more
164  if (sba.calcRMSCost() > 4.0)
165  sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY); // do more
166  }
167  }
168 }
169 
170 void SBANode::publishTopics(/*const ros::TimerEvent& event*/)
171 {
172  // Visualization
173  if (cam_marker_pub.getNumSubscribers() > 0 || point_marker_pub.getNumSubscribers() > 0)
174  {
175  drawGraph(sba, cam_marker_pub, point_marker_pub);
176  }
177 }
178 
180 {
181  // Advertise visualization topics.
182  cam_marker_pub = n.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
183  point_marker_pub = n.advertise<visualization_msgs::Marker>("/sba/points", 1);
184 
185  //timer_sba = n.createTimer(ros::Duration(5.0), &SBANode::doSBA, this);
186  //timer_vis = n.createTimer(ros::Duration(1.0), &SBANode::publishTopics, this);
187 
188  // Subscribe to topics.
189  frame_sub = n.subscribe<sba::Frame>("/sba/frames", 5000, &SBANode::addFrame, this);
190 
191  sba.useCholmod(true);
192 
193  printf("[SBA] Initialization complete.\n");
194 }
195 
196 int main(int argc, char** argv)
197 {
198  ros::init(argc, argv, "sba_node");
199  SBANode sbanode;
200 
201  ros::spin();
202 }
203 
#define SBA_SPARSE_CHOLESKY
Definition: sba.h:67
ros::Publisher cam_marker_pub
Definition: sba_node.cpp:25
void addNode(const sba::CameraNode &msg)
Definition: sba_node.cpp:85
int main(int argc, char **argv)
Definition: sba_node.cpp:196
SysSBA holds a set of nodes and points for sparse bundle adjustment.
Definition: sba.h:75
ros::Timer timer_vis
Definition: sba_node.cpp:29
void addProj(const sba::Projection &msg)
Definition: sba_node.cpp:112
void doSBA()
Definition: sba_node.cpp:138
std::map< unsigned int, unsigned int > node_indices
Definition: sba_node.cpp:35
void drawGraph(const SysSBA &sba, const ros::Publisher &camera_pub, const ros::Publisher &point_pub, int decimation=1, int bicolor=0)
ros::Publisher point_marker_pub
Definition: sba_node.cpp:26
void addPoint(const sba::WorldPoint &msg)
Definition: sba_node.cpp:104
SysSBA sba
Definition: sba_node.cpp:22
ros::NodeHandle n
Definition: sba_node.cpp:23
Definition: bpcg.h:60
ros::Subscriber frame_sub
Definition: sba_node.cpp:24
void publishTopics()
Definition: sba_node.cpp:170
ros::Timer timer_sba
Definition: sba_node.cpp:28
void addFrame(const sba::Frame::ConstPtr &msg)
Definition: sba_node.cpp:46
std::map< unsigned int, unsigned int > point_indices
Definition: sba_node.cpp:32


sparse_bundle_adjustment
Author(s):
autogenerated on Fri Apr 3 2020 03:30:53