// 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);
}
}
}
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 SBANode::addProj(const sba::Projection& msg)
{
int camindex = node_indices[msg.camindex];
int pointindex = point_indices[msg.pointindex];
Vector3d keypoint(msg.u, msg.v, msg.d);
bool stereo = msg.stereo;
bool usecovariance = msg.usecovariance;
Eigen::Matrix3d covariance;
covariance << msg.covariance[0], msg.covariance[1], msg.covariance[2],
msg.covariance[3], msg.covariance[4], msg.covariance[5],
msg.covariance[6], msg.covariance[7], msg.covariance[8];
// Make sure it's valid before adding it.
if (pointindex < (int)sba.tracks.size() && camindex < (int)sba.nodes.size())
{
sba.addProj(camindex, pointindex, keypoint, stereo);
if (usecovariance)
sba.setProjCovariance(camindex, pointindex, covariance);
}
else
{
ROS_INFO("Failed to add projection: C: %d, P: %d, Csize: %d, Psize: %d",
camindex, pointindex,(int)sba.nodes.size(),(int)sba.tracks.size());
}
}
int addPointAndProjection(SysSBA& sba, vector<Point, Eigen::aligned_allocator<Point> >& points, int ndi)
{
// Define dimensions of the image.
int maxx = 640;
int maxy = 480;
// Project points into nodes.
for (int i = 0; i < points.size(); i++)
{
double pointnoise = 0.1;
// Add points into the system, and add noise.
// Add up to .5 pixels of noise.
Vector4d temppoint = points[i];
temppoint.x() += pointnoise*(drand48() - 0.5);
temppoint.y() += pointnoise*(drand48() - 0.5);
temppoint.z() += pointnoise*(drand48() - 0.5);
int index = sba.addPoint(temppoint);
Vector3d proj;
calculateProj(sba, points[i], ndi, proj);
// If valid (within the range of the image size), add the stereo
// projection to SBA.
//if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
//{
sba.addStereoProj(ndi, index, proj);
//}
}
return sba.tracks.size() - points.size();
}
void SBANode::addPoint(const sba::WorldPoint& msg)
{
Vector4d point(msg.x, msg.y, msg.z, msg.w);
unsigned int newindex = sba.addPoint(point);
point_indices[msg.index] = newindex;
}
// 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;
}
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();
}
void SBANode::doSBA(/*const ros::TimerEvent& event*/)
{
unsigned int projs = 0;
// For debugging.
for (int i = 0; i < (int)sba.tracks.size(); i++)
{
projs += sba.tracks[i].projections.size();
}
ROS_INFO("SBA Nodes: %d, Points: %d, Projections: %d", (int)sba.nodes.size(),
(int)sba.tracks.size(), projs);
if (sba.nodes.size() > 0)
{
// Copied from vslam.cpp: refine()
sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY);
double cost = sba.calcRMSCost();
if (isnan(cost) || isinf(cost)) // is NaN?
{
ROS_INFO("NaN cost!");
}
else
{
if (sba.calcRMSCost() > 4.0)
sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY); // do more
if (sba.calcRMSCost() > 4.0)
sba.doSBA(10, 1.0e-4, SBA_SPARSE_CHOLESKY); // do more
}
}
}
void SBANode::addNode(const sba::CameraNode& msg)
{
Vector4d trans(msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z, 1.0);
Quaterniond qrot(msg.transform.rotation.w, msg.transform.rotation.x, msg.transform.rotation.y, msg.transform.rotation.z);
frame_common::CamParams cam_params;
cam_params.fx = msg.fx;
cam_params.fy = msg.fy;
cam_params.cx = msg.cx;
cam_params.cy = msg.cy;
cam_params.tx = msg.baseline;
bool fixed = msg.fixed;
unsigned int newindex = sba.addNode(trans, qrot, cam_params, fixed);
node_indices[msg.index] = newindex;
}
int mark_points(SysSBA &sba, Node& ci, vector< Point,Eigen::aligned_allocator<Point> >& Wpts, vector<int> &Wptref,
vector<int> &Wptind)
{
int ntot = 0;
int npts = Wpts.size();
double maxx = 2.0*ci.Kcam(0,2); // cx
double maxy = 2.0*ci.Kcam(1,2); // cy
if (camn == camp)
{
cout << ci.trans.transpose() << endl;
}
for (int i=0; i<npts; ++i)
{
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)
{
if (camn == camp)
{
cout << "pw: " << Wpts[i].transpose() << endl;
cout << "pc: " << qw.transpose() << endl << endl;
}
Wptref[i]++;
if (Wptref[i] == 2)
{
++ntot;
Wptind[i] = sba.tracks.size(); // index into Wpts
// cout << ntot << " " << sba.points.size() << endl;
sba.addPoint(Wpts[i]);
}
}
}
}
++camn;
return ntot;
}
size_t PoseEstimator<PointSource,PointTarget>::estimate(const NDTFrame<PointSource>& f0, const NDTFrame<PointTarget>& f1,
const std::vector<cv::DMatch> &matches)
{
// convert keypoints in match to 3d points
std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p0; // homogeneous coordinates
std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > p1;
int nmatch = matches.size();
//srand(getDoubleTime());
// set up data structures for fast processing
// indices to good matches
std::vector<int> m0, m1;
for (int i=0; i<nmatch; i++)
{
m0.push_back(matches[i].queryIdx);
m1.push_back(matches[i].trainIdx);
//std::cout<<m0[i]<<" "<<m1[i]<<std::endl;
}
nmatch = m0.size();
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
// RANSAC loop
//#pragma omp parallel for shared( bestinl )
for (int i=0; i<numRansac; i++)
{
//std::cout << "ransac loop : " << i << std::endl;
// find a candidate
int a=rand()%nmatch;
int b = a;
while (a==b)
b=rand()%nmatch;
int c = a;
while (a==c || b==c)
c=rand()%nmatch;
int i0a = m0[a];
int i0b = m0[b];
int i0c = m0[c];
int i1a = m1[a];
int i1b = m1[b];
int i1c = m1[c];
if (i0a == i0b || i0a == i0c || i0b == i0c ||
i1a == i1b || i1a == i1c || i1b == i1c)
continue;
//std::cout<<a<<" "<<b<<" "<<c<<std::endl;
//std::cout<<i0a<<" "<<i0b<<" "<<i0c<<std::endl;
//std::cout<<i1a<<" "<<i1b<<" "<<i1c<<std::endl;
// get centroids
Eigen::Vector3d p0a = f0.pts[i0a].head(3);
Eigen::Vector3d p0b = f0.pts[i0b].head(3);
Eigen::Vector3d p0c = f0.pts[i0c].head(3);
Eigen::Vector3d p1a = f1.pts[i1a].head(3);
Eigen::Vector3d p1b = f1.pts[i1b].head(3);
Eigen::Vector3d p1c = f1.pts[i1c].head(3);
Eigen::Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
Eigen::Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);
//std::cout<<c0.transpose()<<std::endl;
//std::cout<<c1.transpose()<<std::endl;
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Eigen::Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
// do the SVD thang
Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d V = svd.matrixV();
Eigen::Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Eigen::Vector3d tr = c0-R*c1; // translation
// transformation matrix, 3x4
Eigen::Matrix<double,3,4> tfm;
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.block<3,3>(0,0) = R;
tfm.col(3) = tr;
#if 0
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
Vector3d pt = tfm*f1.pts[m1[i]];
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[m0[i]];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[m0[i]] - ipt.z();
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
{
inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
// inl+=(int)sqrt(ipt.z()/matches[i].distance);
// cout << "matches[i].distance : " << matches[i].distance << endl;
// inl++;
}
}
#endif
int inl = 0;
for (int i=0; i<nmatch; i++)
{
Eigen::Vector3d pt1 = tfm*f1.pts[m1[i]];
Eigen::Vector3d pt0 = f0.pts[m0[i]].head(3);
// double z = fabs(pt1.z() - pt0.z())*0.5;
double z = pt1.z();
double dx = pt1.x() - pt0.x();
double dy = pt1.y() - pt0.y();
double dd = pt1.z() - pt0.z();
if (projectMatches)
{
// The central idea here is to divide by the distance (this is essentially what cam2pix does).
dx = dx / z;
dy = dy / z;
}
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
{
//---- inl+=(int)sqrt(pt0.z()); // clever way to weight closer points
// inl+=(int)sqrt(ipt.z()/matches[i].distance);
// cout << "matches[i].distance : " << matches[i].distance << endl;
inl++;
}
}
//#pragma omp critical
if (inl > bestinl)
{
bestinl = inl;
rot = R;
trans = tr;
// std::cout << "bestinl : " << bestinl << std::endl;
}
}
//printf("Best inliers: %d\n", bestinl);
//printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
std::vector<cv::DMatch> inls; // temporary for current inliers
inliers.clear();
Eigen::Matrix<double,3,4> tfm;
tfm.block<3,3>(0,0) = rot;
tfm.col(3) = trans;
//std::cout<<"f0: "<<f0.pts.size()<<" "<<f0.kpts.size()<<" "<<f0.pc_kpts.size()<<std::endl;
//std::cout<<"f1: "<<f1.pts.size()<<" "<<f1.kpts.size()<<" "<<f1.pc_kpts.size()<<std::endl;
nmatch = matches.size();
for (int i=0; i<nmatch; i++)
{
Eigen::Vector3d pt1 = tfm*f1.pts[matches[i].trainIdx];
//Eigen::Vector3d pt1_unchanged = f1.pts[matches[i].trainIdx].head(3);
//Vector3d pt1 = pt1_unchanged;
#if 0
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[matches[i].queryIdx] - ipt.z();
#endif
Eigen::Vector3d pt0 = f0.pts[matches[i].queryIdx].head(3);
//double z = fabs(pt1.z() - pt0.z())*0.5;
double z = pt1.z();
double dx = pt1.x() - pt0.x();
double dy = pt1.y() - pt0.y();
double dd = pt1.z() - pt0.z();
if (projectMatches)
{
// The central idea here is to divide by the distance (this is essentially what cam2pix does).
dx = dx / z;
dy = dy / z;
}
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
{
if (z < maxDist && z > minDist)
// if (fabs(f0.kpts[matches[i].queryIdx].pt.y - f1.kpts[matches[i].trainIdx].pt.y) > 300)
{
// std::cout << " ---------- " << dx << "," << dy << "," << dd << ",\npt0 " << pt0.transpose() << "\npt1 " << pt1.transpose() << f0.kpts[matches[i].queryIdx].pt << "," <<
// f1.kpts[matches[i].trainIdx].pt << "\n unchanged pt1 " << pt1_unchanged.transpose() << std::endl;
inliers.push_back(matches[i]);
}
}
}
#if 0
// Test with the SBA...
{
// system
SysSBA sba;
sba.verbose = 0;
#if 0
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = f0.pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0); // 2.0
cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
quat = q1;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
}
#endif
// std::cout << std::endl << trans.transpose().head(3) << std::endl << std::endl;
// std::cout << rot << std::endl;
return inliers.size();
}
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();
}
}
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.3; // Baseline
// Define dimensions of the image.
int maxx = 640;
int maxy = 480;
// Create a plane containing a wall of points.
Plane middleplane;
middleplane.resize(3, 2, 10, 5);
//middleplane.rotate(PI/4.0, PI/6.0, 1, 0);
middleplane.rotate(PI/4.0, 1, 0, 1);
middleplane.translate(0.0, 0.0, 5.0);
// Create another plane containing a wall of points.
Plane mp2;
mp2.resize(3, 2, 10, 5);
mp2.rotate(0, 0, 0, 1);
mp2.translate(0.0, 0.0, 4.0);
// Create another plane containing a wall of points.
Plane mp3;
mp3.resize(3, 2, 10, 5);
mp3.rotate(-PI/4.0, 1, 0, 1);
mp3.translate(0.0, 0.0, 4.5);
// Vector containing the true point positions.
vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > normals;
points.insert(points.end(), middleplane.points.begin(), middleplane.points.end());
normals.insert(normals.end(), middleplane.points.size(), middleplane.normal);
points.insert(points.end(), mp2.points.begin(), mp2.points.end());
normals.insert(normals.end(), mp2.points.size(), mp2.normal);
points.insert(points.end(), mp3.points.begin(), mp3.points.end());
normals.insert(normals.end(), mp3.points.size(), mp3.normal);
// Create nodes and add them to the system.
unsigned int nnodes = 2; // Number of nodes.
double path_length = 1.0; // Length of the path the nodes traverse.
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
unsigned int i = 0;
Vector3d inormal0 = middleplane.normal;
Vector3d inormal1 = middleplane.normal;
Vector3d inormal20 = mp2.normal;
Vector3d inormal21 = mp2.normal;
Vector3d inormal30 = mp3.normal;
Vector3d inormal31 = mp3.normal;
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);
#if 1
if (i >= 2)
{
// perturb a little
double tnoise = 0.5; // meters
trans.x() += tnoise*(drand48()-0.5);
trans.y() += tnoise*(drand48()-0.5);
trans.z() += tnoise*(drand48()-0.5);
}
#endif
// Don't rotate.
Quaterniond rot(1, 0, 0, 0);
#if 1
if (i >= 2)
{
// perturb a little
double qnoise = 0.1; // meters
rot.x() += qnoise*(drand48()-0.5);
rot.y() += qnoise*(drand48()-0.5);
rot.z() += qnoise*(drand48()-0.5);
}
#endif
rot.normalize();
// Add a new node to the system.
sys.addNode(trans, rot, cam_params, false);
// set normal
if (i == 0)
{
inormal0 = rot.toRotationMatrix().transpose() * inormal0;
inormal20 = rot.toRotationMatrix().transpose() * inormal20;
inormal30 = rot.toRotationMatrix().transpose() * inormal30;
}
else
{
inormal1 = rot.toRotationMatrix().transpose() * inormal1;
inormal21 = rot.toRotationMatrix().transpose() * inormal21;
inormal31 = rot.toRotationMatrix().transpose() * inormal31;
}
}
double pointnoise = 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() += pointnoise*(drand48() - 0.5);
temppoint.y() += pointnoise*(drand48() - 0.5);
temppoint.z() += pointnoise*(drand48() - 0.5);
sys.addPoint(temppoint);
}
// Each point gets added twice
for (i = 0; i < points.size(); i++)
{
// Add up to .5 points of noise.
Vector4d temppoint = points[i];
temppoint.x() += pointnoise*(drand48() - 0.5);
temppoint.y() += pointnoise*(drand48() - 0.5);
temppoint.z() += pointnoise*(drand48() - 0.5);
sys.addPoint(temppoint);
}
Vector2d proj2d, proj2dp;
Vector3d proj, projp, pc, pcp, baseline, baselinep;
Vector3d imagenormal(0, 0, 1);
Matrix3d covar0;
covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2));
Matrix3d covar;
Quaterniond rotation;
Matrix3d rotmat;
unsigned int midindex = middleplane.points.size();
printf("Normal for Middle Plane: [%f %f %f], index %d -> %d\n", middleplane.normal.x(), middleplane.normal.y(), middleplane.normal.z(), 0, midindex-1);
int nn = points.size();
// Project points into nodes.
for (i = 0; i < points.size(); i++)
{
int k=i;
// Project the point into the node's image coordinate system.
sys.nodes[0].setProjection();
sys.nodes[0].project2im(proj2d, points[k]);
sys.nodes[1].setProjection();
sys.nodes[1].project2im(proj2dp, points[k]);
// Camera coords for right camera
baseline << sys.nodes[0].baseline, 0, 0;
pc = sys.nodes[0].Kcam * (sys.nodes[0].w2n*points[k] - baseline);
proj.head<2>() = proj2d;
proj(2) = pc(0)/pc(2);
baseline << sys.nodes[1].baseline, 0, 0;
pcp = sys.nodes[1].Kcam * (sys.nodes[1].w2n*points[k] - baseline);
projp.head<2>() = proj2dp;
projp(2) = pcp(0)/pcp(2);
// add noise to projections
double prnoise = 0.5; // 0.5 pixels
proj.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5));
projp.head<2>() += Vector2d(prnoise*(drand48() - 0.5),prnoise*(drand48() - 0.5));
// If valid (within the range of the image size), add the stereo
// projection to SBA.
if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
{
// add point cloud shape-holding projections to each node
sys.addStereoProj(0, k, proj);
sys.addStereoProj(1, k+nn, projp);
#ifdef USE_PP
// add exact matches
if (i == 20 || i == 65 || i == 142)
sys.addStereoProj(1, k, projp);
#endif
#ifdef USE_PPL
// add point-plane matches
if (i < midindex)
sys.addPointPlaneMatch(0, k, inormal0, 1, k+nn, inormal1);
else if (i < 2*midindex)
sys.addPointPlaneMatch(0, k, inormal20, 1, k+nn, inormal21);
else
sys.addPointPlaneMatch(0, k, inormal30, 1, k+nn, inormal31);
// sys.addStereoProj(0, k+nn, projp);
// sys.addStereoProj(1, k, proj);
Matrix3d covar;
double cv = 0.01;
covar << cv, 0, 0,
0, cv, 0,
0, 0, cv;
sys.setProjCovariance(0, k+nn, covar);
sys.setProjCovariance(1, k, covar);
#endif
}
else
{
cout << "ERROR! point not in view of nodes" << endl;
//return;
}
}
// Add noise to node position.
double transscale = 2.0;
double rotscale = 0.5;
// 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);
}
}
// test the transform functions
TEST(SBAtest, SimpleSystem)
{
// set up full system
SysSBA sys;
// set of world points
// each row is a point
Matrix<double,23,4> pts;
pts << 0.5, 0.2, 3, 1.0,
1, 0, 2, 1.0,
-1, 0, 2, 1.0,
0, 0.2, 3, 1.0,
1, 1, 2, 1.0,
-1, -1, 2, 1.0,
1, 0.2, 4, 1.0,
0, 1, 2.5, 1.0,
0, -1, 2.5, 1.0,
0.2, 0, 3, 1.0,
-1, 1, 2.5, 1.0,
1, -1, 2.5, 1.0,
0.5, 0.2, 4, 1.0,
0.2, -1.3, 2.5, 1.0,
-0.5, -1, 2.5, 1.0,
0.2, -0.7, 3, 1.0,
-1, 1, 3.5, 1.0,
1, -1, 3.5, 1.0,
0.5, 0.2, 4.6, 1.0,
-1, 0, 4, 1.0,
0, 0, 4, 1.0,
1, 1, 4, 1.0,
-1, -1, 4, 1.0;
for (int i=0; i<pts.rows(); i++)
{
Point pt(pts.row(i));
sys.addPoint(pt);
}
Node::initDr(); // set up fixed matrices
// set of nodes
// three nodes, one at origin, two displaced
CamParams cpars = {300,300,320,240,0.1}; // 300 pix focal length, 0.1 m baseline
Quaternion<double> frq2(AngleAxisd(10*M_PI/180,Vector3d(.2,.3,.4).normalized())); // frame rotation in the world
Vector4d frt2(0.2, -0.4, 1.0, 1.0); // frame position in the world
Quaternion<double> frq3(AngleAxisd(10*M_PI/180,Vector3d(-.2,.1,-.3).normalized())); // frame rotation in the world
Vector4d frt3(-0.2, 0.4, 1.0, 1.0); // frame position in the world
Vector3d b(cpars.tx,0,0);
// set up nodes
Node nd1;
Vector4d qr(0,0,0,1);
nd1.qrot = qr; // no rotation
nd1.trans = Vector4d::Zero(); // or translation
nd1.setTransform(); // set up world2node transform
nd1.setKcam(cpars); // set up node2image projection
#ifdef LOCAL_ANGLES
nd1.setDr(true); // set rotational derivatives
#else
nd1.setDr(false); // set rotational derivatives
#endif
nd1.isFixed = true;
Node nd2;
nd2.qrot = frq2;
cout << "Quaternion: " << nd2.qrot.coeffs().transpose() << endl;
nd2.trans = frt2;
cout << "Translation: " << nd2.trans.transpose() << endl << endl;
nd2.setTransform(); // set up world2node transform
nd2.setKcam(cpars); // set up node2image projection
#ifdef LOCAL_ANGLES
nd2.setDr(true); // set rotational derivatives
#else
nd2.setDr(false); // set rotational derivatives
#endif
nd2.isFixed = false;
Node nd3;
nd3.qrot = frq3;
cout << "Quaternion: " << nd3.qrot.coeffs().transpose() << endl;
nd3.trans = frt3;
cout << "Translation: " << nd3.trans.transpose() << endl << endl;
nd3.setTransform(); // set up world2node transform
nd3.setKcam(cpars); // set up node2image projection
#ifdef LOCAL_ANGLES
nd3.setDr(true); // set rotational derivatives
#else
nd3.setDr(false); // set rotational derivatives
#endif
nd3.isFixed = false;
sys.nodes.push_back(nd1);
sys.nodes.push_back(nd2);
sys.nodes.push_back(nd3);
// set up projections onto nodes
int ind = 0;
double inoise = 0.5;
Vector3d n2;
for(unsigned int i = 0; i < sys.tracks.size(); i++)
{
Point pt = sys.tracks[i].point;
ProjMap &prjs = sys.tracks[i].projections; // new point track
Proj prj;
prj.isValid = true;
prj.stereo = true;
Vector2d ipt;
Vector3d ipt3,pc;
n2.setRandom();
nd1.project2im(ipt,pt); // set up projection measurement
prj.ndi = 0; // nd1 index
ipt3.head(2) = ipt;
pc = nd1.Kcam * (nd1.w2n*pt - b); // camera coords for right cam
ipt3(2) = pc(0)/pc(2);
prj.kp = ipt3 + n2*inoise;
prjs[prjs.size()] = prj;
n2.setRandom();
nd2.project2im(ipt,pt); // set up projection measurement
prj.ndi = 1; // nd2 index
ipt3.head(2) = ipt;
pc = nd2.Kcam * (nd2.w2n*pt - b); // camera coords for right cam
ipt3(2) = pc(0)/pc(2);
prj.kp = ipt3 + n2*inoise;
prjs[prjs.size()] = prj;
n2.setRandom();
nd3.project2im(ipt,pt); // set up projection measurement
prj.ndi = 2; // nd3 index
ipt3.head(2) = ipt;
pc = nd3.Kcam * (nd3.w2n*pt - b); // camera coords for right cam
ipt3(2) = pc(0)/pc(2);
prj.kp = ipt3 + n2*inoise;
prjs[prjs.size()] = prj;
//sys.tracksSt.push_back(prjs);
ind++;
}
#ifdef WRITE_GRAPH
writeGraphFile("sba.graph",sys);
#endif
double qnoise = 10*M_PI/180; // in radians
double tnoise = 0.1; // in meters
// add random noise to node positions
nd2.qrot.coeffs().head<3>() += qnoise*Vector3d::Random();
nd2.normRot();
cout << "Quaternion: " << nd2.qrot.coeffs().transpose() << endl << endl;
nd2.trans.head<3>() += tnoise*Vector3d::Random();
nd2.setTransform(); // set up world2node transform
nd2.setProjection();
#ifdef LOCAL_ANGLES
nd2.setDr(true); // set rotational derivatives
#else
nd2.setDr(false); // set rotational derivatives
#endif
sys.nodes[1] = nd2; // reset node
nd3.qrot.coeffs().head<3>() += qnoise*Vector3d::Random();
nd3.normRot();
// cout << "Quaternion: " << nd3.qrot.transpose() << endl << endl;
nd3.trans.head<3>() += tnoise*Vector3d::Random();
nd3.setTransform(); // set up world2node transform
nd3.setProjection(); // set up node2image projection
#ifdef LOCAL_ANGLES
nd3.setDr(true); // set rotational derivatives
#else
nd3.setDr(false); // set rotational derivatives
#endif
sys.nodes[2] = nd3; // reset node
#ifdef WRITE_GRAPH
writeGraphFile("sba-with-err.graph",sys);
#endif
#if 0
// set up system, no lambda for here
sys.setupSys(0.0);
ofstream(fd);
fd.open("A.txt");
fd.precision(8); // this is truly inane
fd << fixed;
fd << sys.A << endl;
fd.close();
fd.open("B.txt");
fd.precision(8);
fd << fixed;
fd << sys.B << endl;
fd.close();
#endif
#ifndef LOCAL_ANGLES
sys.useLocalAngles = false;
#endif
sys.nFixed = 1; // number of fixed cameras
sys.doSBA(10,1.0e-3,false);
cout << endl << "Quaternion: " << sys.nodes[1].qrot.coeffs().transpose() << endl;
// normalize output translation
Vector4d frt2a = sys.nodes[1].trans;
double s = frt2.head<3>().norm() / frt2a.head<3>().norm();
frt2a.head<3>() *= s;
cout << "Translation: " << frt2a.transpose() << endl << endl;
cout << "Quaternion: " << sys.nodes[2].qrot.coeffs().transpose() << endl;
Vector4d frt3a = sys.nodes[2].trans;
s = frt3.head<3>().norm() / frt3a.head<3>().norm();
frt3a.head<3>() *= s;
cout << "Translation: " << frt3a.transpose() << endl << endl;
// calculate cost, should be close to zero
double cost = 0.0;
EXPECT_EQ_ABS(cost,0.0,10.0);
// cameras should be close to their original positions,
// adjusted for scale on translations
for (int i=0; i<4; i++)
EXPECT_EQ_ABS(sys.nodes[1].qrot.coeffs()[i],frq2.coeffs()[i],0.01);
for (int i=0; i<4; i++)
EXPECT_EQ_ABS(sys.nodes[2].qrot.coeffs()[i],frq3.coeffs()[i],0.01);
for (int i=0; i<3; i++)
EXPECT_EQ_ABS(frt2a(i),frt2(i),0.01);
for (int i=0; i<3; i++)
EXPECT_EQ_ABS(frt3a(i),frt3(i),0.01);
#if 1
// writing out matrices, 3-node system
// global system
sys.useLocalAngles = false;
nd1.setDr(false);
nd2.setDr(false);
nd3.setDr(false);
sys.setupSys(0.0);
sys.A.block<6,6>(6,0) = sys.A.block<6,6>(0,6).transpose();
writeA("A3g.txt",sys);
// local system
sys.useLocalAngles = true;
nd1.setDr(true);
nd2.setDr(true);
nd3.setDr(true);
sys.setupSys(0.0);
sys.A.block<6,6>(6,0) = sys.A.block<6,6>(0,6).transpose();
writeA("A3l.txt",sys);
#endif
#if 0
// set up 2-node system
sys.nodes.clear();
sys.tracks.clear();
sys.nodes.push_back(nd1);
sys.nodes.push_back(nd2);
// set up projections onto nodes
ind = 0;
for(vector<Point,Eigen::aligned_allocator<Point> >::iterator itr = sys.points.begin(); itr!=sys.points.end(); itr++)
{
Point pt = *itr;
vector<Proj,Eigen::aligned_allocator<Proj> > prjs; // new point track
Proj prj;
prj.isValid = true;
prj.pti = ind;
prj.kpi = ind;
Vector2d ipt;
n2.setRandom();
nd1.project2im(ipt,pt); // set up projection measurement
prj.ndi = 0; // nd1 index
prj.kp = ipt + n2*inoise;
prjs.push_back(prj);
n2.setRandom();
nd2.project2im(ipt,pt); // set up projection measurement
prj.ndi = 1; // nd2 index
prj.kp = ipt + n2*inoise;
prjs.push_back(prj);
sys.tracks.push_back(prjs);
ind++;
}
sys.doSBA(3);
// writing out matrices, 2-node system
// global system
sys.useLocalAngles = false;
nd1.setDr(false);
nd2.setDr(false);
sys.setupSys(0.0);
sys.writeA("A2g.txt");
// local system
sys.useLocalAngles = true;
nd1.setDr(true);
nd2.setDr(true);
sys.setupSys(0.0);
sys.writeA("A2l.txt");
#endif
}
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 = -30; // 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.
Plane middleplane;
middleplane.resize(3, 2, 10, 5);
middleplane.translate(0.0, 0.0, 7.0);
Plane leftplane;
leftplane.resize(1, 2, 6, 12);
// leftplane.rotate(-PI/4.0, 0, 1, 0);
leftplane.translate(0, 0, 7.0);
Plane rightplane;
rightplane.resize(1, 2, 6, 12);
// rightplane.rotate(PI/4.0, 0, 1, 0);
rightplane.translate(2, 0, 7.0);
Plane topplane;
topplane.resize(1, 1.5, 6, 12);
// topplane.rotate(PI/4.0, 1, 0, 0);
topplane.translate(2, 0, 7.0);
// Vector containing the true point positions.
rightplane.normal = rightplane.normal;
vector<Point, Eigen::aligned_allocator<Point> > points;
vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > normals;
points.insert(points.end(), middleplane.points.begin(), middleplane.points.end());
normals.insert(normals.end(), middleplane.points.size(), middleplane.normal);
points.insert(points.end(), leftplane.points.begin(), leftplane.points.end());
normals.insert(normals.end(), leftplane.points.size(), leftplane.normal);
points.insert(points.end(), rightplane.points.begin(), rightplane.points.end());
normals.insert(normals.end(), rightplane.points.size(), rightplane.normal);
points.insert(points.end(), topplane.points.begin(), topplane.points.end());
normals.insert(normals.end(), topplane.points.size(), topplane.normal);
// Create nodes and add them to the system.
unsigned int nnodes = 2; // Number of nodes.
double path_length = 0.5; // Length of the path the nodes traverse.
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
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);
#if 1
if (i >= 0)
{
// perturb a little
double tnoise = 0.5; // meters
trans.x() += tnoise*(drand48()-0.5);
trans.y() += tnoise*(drand48()-0.5);
trans.z() += tnoise*(drand48()-0.5);
}
#endif
// Don't rotate.
Quaterniond rot(1, 0, 0, 0);
#if 1
if (i >= 0)
{
// perturb a little
double qnoise = 0.1; // meters
rot.x() += qnoise*(drand48()-0.5);
rot.y() += qnoise*(drand48()-0.5);
rot.z() += qnoise*(drand48()-0.5);
}
#endif
rot.normalize();
// Add a new node to the system.
sys.addNode(trans, rot, cam_params, false);
}
double pointnoise = 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() += pointnoise*(drand48() - 0.5);
temppoint.y() += pointnoise*(drand48() - 0.5);
temppoint.z() += pointnoise*(drand48() - 0.5);
sys.addPoint(temppoint);
}
Vector2d proj2d;
Vector3d proj, pc, baseline;
Vector3d imagenormal(0, 0, 1);
Matrix3d covar0;
covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2));
Matrix3d covar;
Quaterniond rotation;
Matrix3d rotmat;
unsigned int midindex = middleplane.points.size();
unsigned int leftindex = midindex + leftplane.points.size();
unsigned int rightindex = leftindex + rightplane.points.size();
printf("Normal for Middle Plane: [%f %f %f], index %d -> %d\n", middleplane.normal.x(), middleplane.normal.y(), middleplane.normal.z(), 0, midindex-1);
printf("Normal for Left Plane: [%f %f %f], index %d -> %d\n", leftplane.normal.x(), leftplane.normal.y(), leftplane.normal.z(), midindex, leftindex-1);
printf("Normal for Right Plane: [%f %f %f], index %d -> %d\n", rightplane.normal.x(), rightplane.normal.y(), rightplane.normal.z(), leftindex, rightindex-1);
// 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(proj2d, points[i]);
// Camera coords for right camera
baseline << sys.nodes[j].baseline, 0, 0;
pc = sys.nodes[j].Kcam * (sys.nodes[j].w2n*points[i] - baseline);
proj.head<2>() = proj2d;
proj(2) = pc(0)/pc(2);
// If valid (within the range of the image size), add the stereo
// projection to SBA.
if (proj.x() > 0 && proj.x() < maxx && proj.y() > 0 && proj.y() < maxy)
{
sys.addStereoProj(j, i, proj);
// Create the covariance matrix:
// image plane normal = [0 0 1]
// wall normal = [0 0 -1]
// covar = (R)T*[0 0 0;0 0 0;0 0 1]*R
rotation.setFromTwoVectors(imagenormal, normals[i]);
rotmat = rotation.toRotationMatrix();
covar = rotmat.transpose()*covar0*rotmat;
// if (!(i % sys.nodes.size() == j))
// sys.setProjCovariance(j, i, covar);
}
}
}
// Add noise to node position.
double transscale = 2.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);
}
}
// assumes cv::Mats are of type <float>
int PoseEstimator::estimate(const matches_t &matches,
const cv::Mat &train_kpts, const cv::Mat &query_kpts,
const cv::Mat &train_pts, const cv::Mat &query_pts,
const cv::Mat &K, const double baseline)
{
// make sure we're using floats
if (train_kpts.depth() != CV_32F ||
query_kpts.depth() != CV_32F ||
train_pts.depth() != CV_32F ||
query_pts.depth() != CV_32F)
throw std::runtime_error("Expected input to be of floating point (CV_32F)");
int nmatch = matches.size();
cout << endl << "[pe3d] Matches: " << nmatch << endl;
// set up data structures for fast processing
// indices to good matches
std::vector<Eigen::Vector3f> t3d, q3d;
std::vector<Eigen::Vector2f> t2d, q2d;
std::vector<int> m; // keep track of matches
for (int i=0; i<nmatch; i++)
{
const float* ti = train_pts.ptr<float>(matches[i].trainIdx);
const float* qi = query_pts.ptr<float>(matches[i].queryIdx);
const float* ti2 = train_kpts.ptr<float>(matches[i].trainIdx);
const float* qi2 = query_kpts.ptr<float>(matches[i].queryIdx);
// make sure we have points within range; eliminates NaNs as well
if (matches[i].distance < 60 && // TODO: get this out of the estimator
ti[2] < maxMatchRange &&
qi[2] < maxMatchRange)
{
m.push_back(i);
t2d.push_back(Eigen::Vector2f(ti2[0],ti2[1]));
q2d.push_back(Eigen::Vector2f(qi2[0],qi2[1]));
t3d.push_back(Eigen::Vector3f(ti[0],ti[1],ti[2]));
q3d.push_back(Eigen::Vector3f(qi[0],qi[1],qi[2]));
}
}
nmatch = m.size();
cout << "[pe3d] Filtered matches: " << nmatch << endl;
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
Matrix3f Kf;
cv::cv2eigen(K,Kf); // camera matrix
float fb = Kf(0,0)*baseline; // focal length times baseline
float maxInlierXDist2 = maxInlierXDist*maxInlierXDist;
float maxInlierDDist2 = maxInlierDDist*maxInlierDDist;
// set up minimum hyp pixel distance
float minpdist = minPDist * Kf(0,2) * 2.0;
// RANSAC loop
int numchoices = 1 + numRansac / 10;
for (int ii=0; ii<numRansac; ii++)
{
// find a candidate
int k;
int a=rand()%nmatch;
int b;
for (k=0; k<numchoices; k++)
{
b=rand()%nmatch;
if (a!=b && (t2d[a]-t2d[b]).norm() > minpdist
&& (q2d[a]-q2d[b]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices) continue;
int c;
for (k=0; k<numchoices+numchoices; k++)
{
c=rand()%nmatch;
if (c!=b && c!=a && (t2d[a]-t2d[c]).norm() > minpdist
&& (t2d[b]-t2d[c]).norm() > minpdist
&& (q2d[a]-q2d[c]).norm() > minpdist
&& (q2d[b]-q2d[c]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices+numchoices) continue;
// get centroids
Vector3f p0a = t3d[a];
Vector3f p0b = t3d[b];
Vector3f p0c = t3d[c];
Vector3f p1a = q3d[a];
Vector3f p1b = q3d[b];
Vector3f p1c = q3d[c];
Vector3f c0 = (p0a+p0b+p0c)*(1.0/3.0);
Vector3f c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3f Hf = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
Matrix3d H = Hf.cast<double>();
#if 0
cout << p0a.transpose() << endl;
cout << p0b.transpose() << endl;
cout << p0c.transpose() << endl;
#endif
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
// cout << "[PE test] R: " << endl << R << endl;
// cout << "[PE test] t: " << tr.transpose() << endl;
Vector3f trf = tr.cast<float>(); // convert to floats
Matrix3f Rf = R.cast<float>();
Rf = Kf*Rf;
trf = Kf*trf;
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
const Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2
&& dd*dd < maxInlierDDist2)
// inl+=(int)fsqrt(ipt.z()); // clever way to weight closer points
inl++;
}
if (inl > bestinl)
{
bestinl = inl;
trans = tr.cast<float>(); // convert to floats
rot = R.cast<float>();
}
}
cout << "[pe3d] Best inliers: " << bestinl << endl;
// printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
matches_t inls; // temporary for current inliers
inliers.clear();
Matrix3f Rf = Kf*rot;
Vector3f trf = Kf*trans;
// cout << "[pe3e] R: " << endl << rot << endl;
cout << "[pe3d] t: " << trans.transpose() << endl;
AngleAxisf aa;
aa.fromRotationMatrix(rot);
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
for (int i=0; i<nmatch; i++)
{
Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inls.push_back(matches[m[i]]);
}
cout << "[pe3d] Final inliers: " << inls.size() << endl;
// polish with SVD
if (polish)
{
Matrix3d Rd = rot.cast<double>();
Vector3d trd = trans.cast<double>();
StereoPolish pol(5,false);
pol.polish(inls,train_kpts,query_kpts,train_pts,query_pts,K,baseline,
Rd,trd);
AngleAxisd aa;
aa.fromRotationMatrix(Rd);
cout << "[pe3d] Polished t: " << trd.transpose() << endl;
cout << "[pe3d] Polished AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
int num = inls.size();
// get centroids
Vector3f c0(0,0,0);
Vector3f c1(0,0,0);
for (int i=0; i<num; i++)
{
c0 += t3d[i];
c1 += q3d[i];
}
c0 = c0 / (float)num;
c1 = c1 / (float)num;
// subtract out and create H matrix
Matrix3f Hf;
Hf.setZero();
for (int i=0; i<num; i++)
{
Vector3f p0 = t3d[i]-c0;
Vector3f p1 = q3d[i]-c1;
Hf += p1*p0.transpose();
}
Matrix3d H = Hf.cast<double>();
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
aa.fromRotationMatrix(R);
cout << "[pe3d] t: " << tr.transpose() << endl;
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
#if 0
// system
SysSBA sba;
sba.verbose = 0;
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = query_pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0);
// cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
#if 0
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
#endif
}
inliers = inls;
return (int)inls.size();
}
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);
}
}
void setupSBA(SysSBA &sba)
{
// 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 = -30; // Baseline (no baseline since this is monocular)
// Create a plane containing a wall of points.
Plane plane0, plane1;
plane0.resize(3, 2, 10, 5);
plane1 = plane0;
plane1.translate(0.1, 0.05, 0.0);
plane1.rotate(PI/4.0, 1, 0, 0);
plane1.translate(0.0, 0.0, 7.0);
plane0.rotate(PI/4.0, 1, 0, 0);
plane0.translate(0.0, 0.0, 7.0);
//plane1.translate(0.05, 0.0, 0.05);
// Create nodes and add them to the system.
unsigned int nnodes = 2; // Number of nodes.
double path_length = 2; // Length of the path the nodes traverse.
// Set the random seed.
unsigned short seed = (unsigned short)time(NULL);
seed48(&seed);
for (int 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);
#if 0
if (i >= 0)
{
// perturb a little
double tnoise = 0.5; // meters
trans.x() += tnoise*(drand48()-0.5);
trans.y() += tnoise*(drand48()-0.5);
trans.z() += tnoise*(drand48()-0.5);
}
#endif
// Don't rotate.
Quaterniond rot(1, 0, 0, 0);
#if 0
if (i > 0)
{
// perturb a little
double qnoise = 0.1; // meters
rot.x() += qnoise*(drand48()-0.5);
rot.y() += qnoise*(drand48()-0.5);
rot.z() += qnoise*(drand48()-0.5);
}
#endif
rot.normalize();
// Add a new node to the system.
sba.addNode(trans, rot, cam_params, false);
}
Vector3d imagenormal(0, 0, 1);
Matrix3d covar0;
covar0 << sqrt(imagenormal(0)), 0, 0, 0, sqrt(imagenormal(1)), 0, 0, 0, sqrt(imagenormal(2));
Matrix3d covar;
Quaterniond rotation;
Matrix3d rotmat;
// Project points into nodes.
addPointAndProjection(sba, plane0.points, 0);
addPointAndProjection(sba, plane1.points, 1);
int offset = plane0.points.size();
Vector3d normal0 = sba.nodes[0].qrot.toRotationMatrix().transpose()*plane0.normal;
Vector3d normal1 = sba.nodes[1].qrot.toRotationMatrix().transpose()*plane1.normal;
printf("Normal: %f %f %f -> %f %f %f\n", plane0.normal.x(), plane0.normal.y(), plane0.normal.z(), normal0.x(), normal0.y(), normal0.z());
printf("Normal: %f %f %f -> %f %f %f\n", plane1.normal.x(), plane1.normal.y(), plane1.normal.z(), normal1.x(), normal1.y(), normal1.z());
for (int i = 0; i < plane0.points.size(); i++)
{
sba.addPointPlaneMatch(0, i, normal0, 1, i+offset, normal1);
Matrix3d covar;
covar << 0.1, 0, 0,
0, 0.1, 0,
0, 0, 0.1;
sba.setProjCovariance(0, i+offset, covar);
sba.setProjCovariance(1, i, covar);
}
// Add noise to node position.
double transscale = 1.0;
double rotscale = 0.1;
// Don't actually add noise to the first node, since it's fixed.
for (int i = 1; i < sba.nodes.size(); i++)
{
Vector4d temptrans = sba.nodes[i].trans;
Quaterniond tempqrot = sba.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();
sba.nodes[i].trans = temptrans;
sba.nodes[i].qrot = tempqrot;
// These methods should be called to update the node.
sba.nodes[i].normRot();
sba.nodes[i].setTransform();
sba.nodes[i].setProjection();
sba.nodes[i].setDr(true);
}
}
int PoseEstimator3d::estimate(const Frame& f0, const Frame& f1,
const std::vector<cv::DMatch> &matches)
{
// convert keypoints in match to 3d points
std::vector<Vector4d, aligned_allocator<Vector4d> > p0; // homogeneous coordinates
std::vector<Vector4d, aligned_allocator<Vector4d> > p1;
int nmatch = matches.size();
// set up data structures for fast processing
// indices to good matches
vector<int> m0, m1;
for (int i=0; i<nmatch; i++)
{
if (f0.disps[matches[i].queryIdx] > minMatchDisp &&
f1.disps[matches[i].trainIdx] > minMatchDisp)
{
m0.push_back(matches[i].queryIdx);
m1.push_back(matches[i].trainIdx);
}
}
nmatch = m0.size();
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
// RANSAC loop
#pragma omp parallel for shared( bestinl )
for (int i=0; i<numRansac; i++)
{
// find a candidate
int a=rand()%nmatch;
int b = a;
while (a==b)
b=rand()%nmatch;
int c = a;
while (a==c || b==c)
c=rand()%nmatch;
int i0a = m0[a];
int i0b = m0[b];
int i0c = m0[c];
int i1a = m1[a];
int i1b = m1[b];
int i1c = m1[c];
if (i0a == i0b || i0a == i0c || i0b == i0c ||
i1a == i1b || i1a == i1c || i1b == i1c)
continue;
// get centroids
Vector3d p0a = f0.pts[i0a].head<3>();
Vector3d p0b = f0.pts[i0b].head<3>();
Vector3d p0c = f0.pts[i0c].head<3>();
Vector3d p1a = f1.pts[i1a].head<3>();
Vector3d p1b = f1.pts[i1b].head<3>();
Vector3d p1c = f1.pts[i1c].head<3>();
Vector3d c0 = (p0a+p0b+p0c)*(1.0/3.0);
Vector3d c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
#if 0
cout << p0a.transpose() << endl;
cout << p0b.transpose() << endl;
cout << p0c.transpose() << endl;
#endif
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d tr = c0-R*c1; // translation
// cout << "[PE test] R: " << endl << R << endl;
// cout << "[PE test] t: " << tr.transpose() << endl;
#if 0
R << 0.9997, 0.002515, 0.02418,
-0.002474, .9999, -0.001698,
-0.02418, 0.001638, 0.9997;
tr << -0.005697, -0.01753, 0.05666;
R = R.transpose();
tr = -R*tr;
#endif
// transformation matrix, 3x4
Matrix<double,3,4> tfm;
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.block<3,3>(0,0) = R;
tfm.col(3) = tr;
// cout << "[PE test] T: " << endl << tfm << endl;
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
Vector3d pt = tfm*f1.pts[m1[i]];
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[m0[i]];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[m0[i]] - ipt.z();
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inl+=(int)sqrt(ipt.z()); // clever way to weight closer points
// inl++;
}
#pragma omp critical
if (inl > bestinl)
{
bestinl = inl;
rot = R;
trans = tr;
}
}
// printf("Best inliers: %d\n", bestinl);
// printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
vector<cv::DMatch> inls; // temporary for current inliers
inliers.clear();
Matrix<double,3,4> tfm;
tfm.block<3,3>(0,0) = rot;
tfm.col(3) = trans;
nmatch = matches.size();
for (int i=0; i<nmatch; i++)
{
if (!f0.goodPts[matches[i].queryIdx] || !f1.goodPts[matches[i].trainIdx])
continue;
Vector3d pt = tfm*f1.pts[matches[i].trainIdx];
Vector3d ipt = f0.cam2pix(pt);
const cv::KeyPoint &kp = f0.kpts[matches[i].queryIdx];
double dx = kp.pt.x - ipt.x();
double dy = kp.pt.y - ipt.y();
double dd = f0.disps[matches[i].queryIdx] - ipt.z();
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inls.push_back(matches[i]);
}
#if 0
cout << endl << trans.transpose().head(3) << endl << endl;
cout << rot << endl;
#endif
// polish with stereo sba
if (polish)
{
// system
SysSBA sba;
sba.verbose = 0;
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = f0.pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0);
// cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
#if 0
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
}
return (int)inliers.size();
}