本文整理汇总了C++中SysSBA::addPointPlaneMatch方法的典型用法代码示例。如果您正苦于以下问题：C++ SysSBA::addPointPlaneMatch方法的具体用法？C++ SysSBA::addPointPlaneMatch怎么用？C++ SysSBA::addPointPlaneMatch使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类SysSBA的用法示例。
在下文中一共展示了SysSBA::addPointPlaneMatch方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: setupSBA
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);
}
//.........这里部分代码省略.........
示例2: setupSBA
//.........这里部分代码省略.........
// 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);
}
}