sba_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ros/time.h>
00003 
00004 // Messages
00005 #include <sba/Frame.h>
00006 #include <visualization_msgs/Marker.h>
00007 #include <sba/sba_file_io.h>
00008 
00009 #include <sba/sba.h>
00010 #include <sba/visualization.h>
00011 
00012 #include <map>
00013 
00014 #include <ros/callback_queue.h>
00015 
00016 
00017 using namespace sba;
00018 
00019 class SBANode
00020 {
00021   public:
00022     SysSBA sba;
00023     ros::NodeHandle n;
00024     ros::Subscriber frame_sub;
00025     ros::Publisher cam_marker_pub;
00026     ros::Publisher point_marker_pub;
00027     
00028     ros::Timer timer_sba;
00029     ros::Timer timer_vis;
00030     
00031     // Mapping from external point index to internal (sba) point index
00032     std::map<unsigned int, unsigned int> point_indices;
00033     
00034     // Mapping from external node index to internal (sba) node index
00035     std::map<unsigned int, unsigned int> node_indices;
00036     
00037     void addFrame(const sba::Frame::ConstPtr& msg);
00038     void addNode(const sba::CameraNode& msg);
00039     void addPoint(const sba::WorldPoint& msg);
00040     void addProj(const sba::Projection& msg);
00041     void doSBA(/*const ros::TimerEvent& event*/);
00042     void publishTopics(/*const ros::TimerEvent& event*/);
00043     SBANode();
00044 };
00045     
00046 void SBANode::addFrame(const sba::Frame::ConstPtr& msg)
00047 {
00048   unsigned int i = 0;
00049   
00050   ros::Time beginning = ros::Time::now();
00051   
00052   // Add all nodes
00053   for (i=0; i < msg->nodes.size(); i++)
00054   {
00055     addNode(msg->nodes[i]);
00056   }
00057   
00058   // Add all points
00059   for (i=0; i < msg->points.size(); i++)
00060   {
00061     addPoint(msg->points[i]);
00062   }
00063   
00064   // Add all projections
00065   for (i=0; i < msg->projections.size(); i++)
00066   { 
00067     addProj(msg->projections[i]);
00068   }
00069   
00070   publishTopics();
00071   
00072   // Do SBA Every 10 frames.
00073   if (sba.nodes.size() % 5 == 0)
00074   {
00075     doSBA();
00076   }
00077   
00078   ros::Time end = ros::Time::now();
00079   
00080   printf("[SBA] Added frame with %u nodes, %u points, and %u projections (%f s)\n", 
00081           (unsigned int)msg->nodes.size(), (unsigned int)msg->points.size(), 
00082           (unsigned int)msg->projections.size(), (end-beginning).toSec());
00083 }
00084 
00085 void SBANode::addNode(const sba::CameraNode& msg)
00086 {
00087   Vector4d trans(msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z, 1.0);
00088   Quaterniond qrot(msg.transform.rotation.w, msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z);
00089   
00090   frame_common::CamParams cam_params;
00091   cam_params.fx = msg.fx;
00092   cam_params.fy = msg.fy;
00093   cam_params.cx = msg.cx;
00094   cam_params.cy = msg.cy;
00095   cam_params.tx = msg.baseline;
00096   
00097   bool fixed = msg.fixed;
00098   
00099   unsigned int newindex = sba.addNode(trans, qrot, cam_params, fixed);
00100   
00101   node_indices[msg.index] = newindex;  
00102 }
00103 
00104 void SBANode::addPoint(const sba::WorldPoint& msg)
00105 {
00106   Vector4d point(msg.x, msg.y, msg.z, msg.w);
00107   unsigned int newindex = sba.addPoint(point);
00108   
00109   point_indices[msg.index] = newindex;
00110 }
00111 
00112 void SBANode::addProj(const sba::Projection& msg)
00113 {
00114   int camindex = node_indices[msg.camindex];
00115   int pointindex = point_indices[msg.pointindex];
00116   Vector3d keypoint(msg.u, msg.v, msg.d);
00117   bool stereo = msg.stereo;
00118   bool usecovariance = msg.usecovariance;
00119   Eigen::Matrix3d covariance;
00120   covariance << msg.covariance[0], msg.covariance[1], msg.covariance[2],
00121                 msg.covariance[3], msg.covariance[4], msg.covariance[5],
00122                 msg.covariance[6], msg.covariance[7], msg.covariance[8];
00123   
00124   // Make sure it's valid before adding it.
00125   if (pointindex < (int)sba.tracks.size() && camindex < (int)sba.nodes.size())
00126   {
00127     sba.addProj(camindex, pointindex, keypoint, stereo);
00128     if (usecovariance)
00129       sba.setProjCovariance(camindex, pointindex, covariance);
00130   }
00131   else
00132   {
00133     ROS_INFO("Failed to add projection: C: %d, P: %d, Csize: %d, Psize: %d", 
00134             camindex, pointindex,(int)sba.nodes.size(),(int)sba.tracks.size());       
00135   }
00136 }
00137 
00138 void SBANode::doSBA(/*const ros::TimerEvent& event*/)
00139 {
00140   unsigned int projs = 0;
00141   // For debugging.
00142   for (int i = 0; i < (int)sba.tracks.size(); i++)
00143   {
00144     projs += sba.tracks[i].projections.size();
00145   }
00146   ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sba.nodes.size(),
00147     (int)sba.tracks.size(), projs);
00148   
00149   if (sba.nodes.size() > 0)
00150   {
00151     // Copied from vslam.cpp: refine()
00152     sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);
00153     
00154     double cost = sba.calcRMSCost();
00155     
00156     if (isnan(cost) || isinf(cost)) // is NaN?
00157     {
00158       ROS_INFO("NaN cost!");  
00159     }
00160     else
00161     { 
00162       if (sba.calcRMSCost() > 4.0)
00163         sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);  // do more
00164       if (sba.calcRMSCost() > 4.0)
00165         sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);  // do more
00166     }
00167   }
00168 }
00169 
00170 void SBANode::publishTopics(/*const ros::TimerEvent& event*/)
00171 {
00172   // Visualization
00173   if (cam_marker_pub.getNumSubscribers() > 0 || point_marker_pub.getNumSubscribers() > 0)
00174   { 
00175      drawGraph(sba, cam_marker_pub, point_marker_pub);
00176   }
00177 }
00178 
00179 SBANode::SBANode()
00180 {
00181   // Advertise visualization topics.
00182   cam_marker_pub = n.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
00183   point_marker_pub = n.advertise<visualization_msgs::Marker>("/sba/points", 1);
00184 
00185   //timer_sba = n.createTimer(ros::Duration(5.0), &SBANode::doSBA, this);
00186   //timer_vis = n.createTimer(ros::Duration(1.0), &SBANode::publishTopics, this);
00187   
00188   // Subscribe to topics.
00189   frame_sub = n.subscribe<sba::Frame>("/sba/frames", 5000, &SBANode::addFrame, this);
00190   
00191   sba.useCholmod(true);
00192   
00193   printf("[SBA] Initialization complete.\n");
00194 }
00195 
00196 int main(int argc, char** argv)
00197 {
00198   ros::init(argc, argv, "sba_node");
00199   SBANode sbanode;
00200 
00201   ros::spin();
00202 }
00203 


sba
Author(s): Kurt Konolige, Helen Oleynikova
autogenerated on Thu Jan 2 2014 12:12:09