// add measurements, assuming ref counts present
// <cind> is the camera/node index
int add_points(SysSBA &sba, double inoise, int cind,
vector< Vector4d,Eigen::aligned_allocator<Vector4d> >& Wpts, vector<int> &Wptref,
vector<int> &Wptind)
{
int ntot = 0;
double inoise2 = inoise*0.5;
int npts = Wpts.size();
Node &ci = sba.nodes[cind];
double maxx = 2.0*ci.Kcam(0,2); // cx
double maxy = 2.0*ci.Kcam(1,2); // cy
for (int i=0; i<npts; ++i)
{
if (Wptref[i] < 2) continue; // have to have at least two points
Vector2d qi;
Vector3d qw;
qw = ci.w2n * Wpts[i]; // point in camera coords
if (qw[2] > near && qw[2] < far)
{
ci.project2im(qi,Wpts[i]); // point in image coords
if (qi[0] > 0.5 && qi[0] < maxx &&
qi[1] > 0.5 && qi[1] < maxy)
{
qi[0] += drand48() * inoise - inoise2; // add noise now
qi[1] += drand48() * inoise - inoise2;
// cout << "Cam and pt indices: " << cind << " " << Wptind[i] << endl;
sba.addMonoProj(cind, Wptind[i], qi);
++ntot;
}
}
}
return ntot;
}
// add point to camera set
// <cinds> is a vector of camera node indices
void
add_sphere_points(SysSBA &sba, double inoise, set<int> &cinds, int ncpts)
{
double inoise2 = inoise*0.5;
for (int i=0; i<ncpts; ++i)
{
// make random point in 0.5m sphere
Vector4d pt;
pt[0] = drand48() - 0.5;
pt[1] = drand48() - 0.5;
pt[2] = drand48() - 0.5;
pt[3] = 1.0;
int pti = sba.addPoint(pt);
// now add projections to each camera
set<int>::iterator it;
for (it=cinds.begin(); it!=cinds.end(); ++it)
{
Node &nd = sba.nodes[*it];
Vector2d qi;
Vector3d qw;
qw = nd.w2n * pt; // point in camera coords
nd.project2im(qi,pt); // point in image coords
qi[0] += drand48() * inoise - inoise2; // add noise now
qi[1] += drand48() * inoise - inoise2;
// cout << "Cam and pt indices: " << cind << " " << Wptind[i] << endl;
sba.addMonoProj(*it, pti, qi);
}
}
}
void setupSBA(SysSBA &sys)
{
// Create camera parameters.
frame_common::CamParams cam_params;
cam_params.fx = 430; // Focal length in x
cam_params.fy = 430; // Focal length in y
cam_params.cx = 320; // X position of principal point
cam_params.cy = 240; // Y position of principal point
cam_params.tx = 0; // Baseline (no baseline since this is monocular)
// Define dimensions of the image.
int maxx = 640;
int maxy = 480;
// Create a plane containing a wall of points.
int npts_x = 10; // Number of points on the plane in x
int npts_y = 5; // Number of points on the plane in y
double plane_width = 5; // Width of the plane on which points are positioned (x)
double plane_height = 2.5; // Height of the plane on which points are positioned (y)
double plane_distance = 5; // Distance of the plane from the cameras (z)
// Vector containing the true point positions.
vector<Point, Eigen::aligned_allocator<Point> > points;
for (int ix = 0; ix < npts_x ; ix++)
{
for (int iy = 0; iy < npts_y ; iy++)
{
// Create a point on the plane in a grid.
points.push_back(Point(plane_width/npts_x*(ix+.5), -plane_height/npts_y*(iy+.5), plane_distance, 1.0));
}
}
// Create nodes and add them to the system.
unsigned int nnodes = 5; // Number of nodes.
double path_length = 3; // Length of the path the nodes traverse.
unsigned int i = 0, j = 0;
for (i = 0; i < nnodes; i++)
{
// Translate in the x direction over the node path.
Vector4d trans(i/(nnodes-1.0)*path_length, 0, 0, 1);
// Don't rotate.
Quaterniond rot(1, 0, 0, 0);
rot.normalize();
// Add a new node to the system.
sys.addNode(trans, rot, cam_params, false);
}
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
double ptscale = 1.0;
// Add points into the system, and add noise.
for (i = 0; i < points.size(); i++)
{
// Add up to .5 points of noise.
Vector4d temppoint = points[i];
temppoint.x() += ptscale*(drand48() - 0.5);
temppoint.y() += ptscale*(drand48() - 0.5);
temppoint.z() += ptscale*(drand48() - 0.5);
sys.addPoint(temppoint);
}
Vector2d proj;
// Project points into nodes.
for (i = 0; i < points.size(); i++)
{
for (j = 0; j < sys.nodes.size(); j++)
{
// Project the point into the node's image coordinate system.
sys.nodes[j].setProjection();
sys.nodes[j].project2im(proj, points[i]);
// If valid (within the range of the image size), add the monocular
// projection to SBA.
if (proj.x() > 0 && proj.x() < maxx-1 && proj.y() > 0 && proj.y() < maxy-1)
{
sys.addMonoProj(j, i, proj);
//printf("Adding projection: Node: %d Point: %d Proj: %f %f\n", j, i, proj.x(), proj.y());
}
}
}
// Add noise to node position.
double transscale = 1.0;
double rotscale = 0.2;
// Don't actually add noise to the first node, since it's fixed.
for (i = 1; i < sys.nodes.size(); i++)
{
Vector4d temptrans = sys.nodes[i].trans;
Quaterniond tempqrot = sys.nodes[i].qrot;
// Add error to both translation and rotation.
temptrans.x() += transscale*(drand48() - 0.5);
temptrans.y() += transscale*(drand48() - 0.5);
temptrans.z() += transscale*(drand48() - 0.5);
tempqrot.x() += rotscale*(drand48() - 0.5);
tempqrot.y() += rotscale*(drand48() - 0.5);
tempqrot.z() += rotscale*(drand48() - 0.5);
tempqrot.normalize();
sys.nodes[i].trans = temptrans;
sys.nodes[i].qrot = tempqrot;
// These methods should be called to update the node.
sys.nodes[i].normRot();
sys.nodes[i].setTransform();
sys.nodes[i].setProjection();
sys.nodes[i].setDr(true);
}
}
