void processSBA(ros::NodeHandle node)
{
// Create publisher topics.
ros::Publisher cam_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
ros::Publisher point_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/points", 1);
ros::spinOnce();
//ROS_INFO("Sleeping for 2 seconds to publish topics...");
ros::Duration(0.2).sleep();
// Create an empty SBA system.
SysSBA sys;
setupSBA(sys);
// Provide some information about the data read in.
unsigned int projs = 0;
// For debugging.
for (int i = 0; i < (int)sys.tracks.size(); i++)
{
projs += sys.tracks[i].projections.size();
}
ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sys.nodes.size(),
(int)sys.tracks.size(), projs);
//ROS_INFO("Sleeping for 5 seconds to publish pre-SBA markers.");
//ros::Duration(5.0).sleep();
// Perform SBA with 10 iterations, an initial lambda step-size of 1e-3,
// and using CSPARSE.
sys.doSBA(20, 1e-4, SBA_SPARSE_CHOLESKY);
int npts = sys.tracks.size();
ROS_INFO("Bad projs (> 10 pix): %d, Cost without: %f",
(int)sys.countBad(10.0), sqrt(sys.calcCost(10.0)/npts));
ROS_INFO("Bad projs (> 5 pix): %d, Cost without: %f",
(int)sys.countBad(5.0), sqrt(sys.calcCost(5.0)/npts));
ROS_INFO("Bad projs (> 2 pix): %d, Cost without: %f",
(int)sys.countBad(2.0), sqrt(sys.calcCost(2.0)/npts));
ROS_INFO("Cameras (nodes): %d, Points: %d",
(int)sys.nodes.size(), (int)sys.tracks.size());
// Publish markers
drawGraph(sys, cam_marker_pub, point_marker_pub);
ros::spinOnce();
//ROS_INFO("Sleeping for 2 seconds to publish post-SBA markers.");
ros::Duration(0.2).sleep();
}
int main(int argc, char **argv)
{
char *fin;
if (argc < 2)
{
cout << "Arguments are: <input filename>" << endl;
return -1;
}
fin = argv[1];
SysSBA sys;
readGraphFile(fin, sys);
// writeGraphFile("sba-out.graph", sys);
double cost = sys.calcCost();
cout << "Initial squared cost: " << cost << endl;
sys.nFixed = 1;
sys.printStats();
sys.csp.useCholmod = true;
// sys.doSBA(10,1e-4,SBA_SPARSE_CHOLESKY);
sys.doSBA(10,1e-4,SBA_BLOCK_JACOBIAN_PCG,1e-8,200);
return 0;
}
void processSBA(ros::NodeHandle node)
{
// Create publisher topics.
ros::Publisher cam_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/cameras", 1);
ros::Publisher point_marker_pub = node.advertise<visualization_msgs::Marker>("/sba/points", 1);
ros::spinOnce();
//ROS_INFO("Sleeping for 2 seconds to publish topics...");
ros::Duration(0.2).sleep();
// Create an empty SBA system.
SysSBA sys;
setupSBA(sys);
// Provide some information about the data read in.
unsigned int projs = 0;
// For debugging.
for (int i = 0; i < (int)sys.tracks.size(); i++)
{
projs += sys.tracks[i].projections.size();
}
ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sys.nodes.size(),
(int)sys.tracks.size(), projs);
//ROS_INFO("Sleeping for 5 seconds to publish pre-SBA markers.");
//ros::Duration(5.0).sleep();
// Perform SBA with 10 iterations, an initial lambda step-size of 1e-3,
// and using CSPARSE.
sys.doSBA(1, 1e-3, SBA_SPARSE_CHOLESKY);
int npts = sys.tracks.size();
ROS_INFO("Bad projs (> 10 pix): %d, Cost without: %f",
(int)sys.countBad(10.0), sqrt(sys.calcCost(10.0)/npts));
ROS_INFO("Bad projs (> 5 pix): %d, Cost without: %f",
(int)sys.countBad(5.0), sqrt(sys.calcCost(5.0)/npts));
ROS_INFO("Bad projs (> 2 pix): %d, Cost without: %f",
(int)sys.countBad(0.5), sqrt(sys.calcCost(2.0)/npts));
ROS_INFO("Cameras (nodes): %d, Points: %d",
(int)sys.nodes.size(), (int)sys.tracks.size());
// Publish markers
drawGraph(sys, cam_marker_pub, point_marker_pub, 1, sys.tracks.size()/2);
ros::spinOnce();
//ROS_INFO("Sleeping for 2 seconds to publish post-SBA markers.");
ros::Duration(0.5).sleep();
for (int j=0; j<30; j+=3)
{
if (!ros::ok())
break;
sys.doSBA(1, 0, SBA_SPARSE_CHOLESKY);
drawGraph(sys, cam_marker_pub, point_marker_pub, 1, sys.tracks.size()/2);
ros::spinOnce();
ros::Duration(0.5).sleep();
}
#ifdef USE_PPL
// reset covariances and continue
for (int i = 0; i < points.size(); i++)
{
int nn = points.size();
Matrix3d covar;
double cv = 0.3;
covar << cv, 0, 0,
0, cv, 0,
0, 0, cv;
sys.setProjCovariance(0, i+nn, covar);
sys.setProjCovariance(1, i, covar);
}
#endif
for (int j=0; j<30; j+=3)
{
if (!ros::ok())
break;
sys.doSBA(1, 0, SBA_SPARSE_CHOLESKY);
drawGraph(sys, cam_marker_pub, point_marker_pub, 1, sys.tracks.size()/2);
ros::spinOnce();
ros::Duration(0.5).sleep();
}
}