void NearPlaneClipper::setup( const Matrix4f& projectionMV )
{
// get normalized near plane
Vector4d plane = projectionMV.get_row( 2 ) +
projectionMV.get_row( 3 );
plane *= 1.0 / sqrt( plane.x()*plane.x() +
plane.y()*plane.y() +
plane.z()*plane.z() );
_nearPlane = Plane_d( plane.x(), plane.y(), plane.z(), plane.w() );
// move near plane slightly ahead, so that when we will draw it, it will not be clipped
_nearPlane.w -= 0.001;
// _nearPlane.w -= 2.0;
}
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 Model::RotateObject(Shape* shape, TreeObject* object, Vector4d rotate)
{
if (!shape)
return;
Vector3d rot(rotate.x(), rotate.y(), rotate.z());
shape->Rotate(rot, rotate.w());
ModelChanged();
}
inline bool cullSphere(const Vector4d& center,
double radius) const
{
return (center.z() - radius > m_nearZ ||
center.z() + radius < m_farZ ||
center.dot(m_planeNormals[0]) < -radius ||
center.dot(m_planeNormals[1]) < -radius ||
center.dot(m_planeNormals[2]) < -radius ||
center.dot(m_planeNormals[3]) < -radius);
}
// Test a point to see if it lies within the frustum defined by
// planes z=nearZ, z=farZ, and the four side planes with specified
// normals.
static inline bool frustumCull(const Vector4d& curvePoint,
double curveBoundingRadius,
double nearZ, double farZ,
const Vector4d viewFrustumPlaneNormals[])
{
return (curvePoint.z() - curveBoundingRadius > nearZ ||
curvePoint.z() + curveBoundingRadius < farZ ||
curvePoint.dot(viewFrustumPlaneNormals[0]) < -curveBoundingRadius ||
curvePoint.dot(viewFrustumPlaneNormals[1]) < -curveBoundingRadius ||
curvePoint.dot(viewFrustumPlaneNormals[2]) < -curveBoundingRadius ||
curvePoint.dot(viewFrustumPlaneNormals[3]) < -curveBoundingRadius);
}
// TODO: Needs more testing
double World::dist_to_wall(const Vector4d& origin, const Vector3d& heading) const
{
list<double> dists;
foreach(const Wall& wall, walls) {
// Direction points p2 -> p1
Vector2d diff = (wall.p1 - wall.p2);
Vector2d d0 = diff.normalized();
Vector2d p0 = wall.p2;
double tmax = diff.x()/d0.x();
// Reduce input ray to 2D
Vector2d d1(origin.x(), origin.z());
Vector2d p1(heading.x(), heading.z());
// Build matrix representation
Matrix<double, 2, 2> a;
Matrix<double, 2, 1> b;
a << d0.y(), d0.x(), d1.y(), d1.x();
b << -p0.y() - p0.x(), -p1.y() - p1.x();
// Solve
Matrix<double, 2, 1> res = a.fullPivLu().solve(b);
if ((a*res).isApprox(b)) {
// Check our solution is within range
// Calculate t for each original equation
double t0, t1;
t0 = res.x()/d0.x();
t1 = res.x()/d1.x();
// t1 is input ray, t0 is wall
if ((t1 < 0) || (t0 < 0) || (t0 > tmax)) {
return -1;
}
// Valid t, find distance from origin to intersect and return
return res.x()/res.normalized().x();
} else {
// Unsolvable (e.g. no intersection)
return -1;
}
}
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);
}
}
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);
}
}
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);
}
}
// Return the GL restart status: true if the last segment of the
// curve was culled and we need to start a new primitive sequence
// with glBegin().
bool renderCubicFaded(bool restartCurve,
const Matrix4d& coeff,
double t0, double t1,
const Vector4f& color,
double fadeStart, double fadeRate,
double curveBoundingRadius,
int depth) const
{
const double dt = (t1 - t0) * InvSubdivisionFactor;
double segmentBoundingRadius = curveBoundingRadius * InvSubdivisionFactor;
#if DEBUG_ADAPTIVE_SPLINE
{
int c = depth % 10;
glColor4f(SplineColors[c][0], SplineColors[c][1], SplineColors[c][2], 1.0f);
++SegmentCounts[depth];
}
#endif
Vector4d lastP = coeff * Vector4d(1.0, t0, t0 * t0, t0 * t0 * t0);
double lastOpacity = (t0 - fadeStart) * fadeRate;
lastOpacity = max(0.0, min(1.0, lastOpacity)); // clamp
for (unsigned int i = 1; i <= SubdivisionFactor; i++)
{
double t = t0 + dt * i;
Vector4d p = coeff * Vector4d(1.0, t, t * t, t * t * t);
double opacity = (t - fadeStart) * fadeRate;
opacity = max(0.0, min(1.0, opacity)); // clamp
double minDistance = max(-m_viewFrustum.nearZ(), abs(p.z()) - segmentBoundingRadius);
if (segmentBoundingRadius >= m_subdivisionThreshold * minDistance)
{
if (m_viewFrustum.cullSphere(p, segmentBoundingRadius))
{
if (!restartCurve)
{
m_vbuf.end();
restartCurve = true;
}
}
else
{
restartCurve = renderCubicFaded(restartCurve,
coeff, t - dt, t,
color,
fadeStart, fadeRate,
segmentBoundingRadius,
depth + 1);
}
}
else
{
#if DEBUG_ADAPTIVE_SPLINE
{
int c = depth % 10;
glColor4f(SplineColors[c][0], SplineColors[c][1], SplineColors[c][2], i % 2 ? 0.25f : 1.0f);
}
#endif
if (restartCurve)
{
m_vbuf.begin();
m_vbuf.vertex(lastP, Vector4f(color.x(), color.y(), color.z(), color.w() * float(lastOpacity)));
restartCurve = false;
}
m_vbuf.vertex(p, Vector4f(color.x(), color.y(), color.z(), color.w() * float(opacity)));
}
lastP = p;
lastOpacity = opacity;
}
return restartCurve;
}
// Return the GL restart status: true if the last segment of the
// curve was culled and we need to start a new primitive sequence
// with glBegin().
bool renderCubic(bool restartCurve,
const Matrix4d& coeff,
double t0, double t1,
double curveBoundingRadius,
int depth) const
{
const double dt = (t1 - t0) * InvSubdivisionFactor;
double segmentBoundingRadius = curveBoundingRadius * InvSubdivisionFactor;
#if DEBUG_ADAPTIVE_SPLINE
{
int c = depth % 10;
glColor4f(SplineColors[c][0], SplineColors[c][1], SplineColors[c][2], 1.0f);
++SegmentCounts[depth];
}
#endif
Vector4d lastP = coeff * Vector4d(1.0, t0, t0 * t0, t0 * t0 * t0);
for (unsigned int i = 1; i <= SubdivisionFactor; i++)
{
double t = t0 + dt * i;
Vector4d p = coeff * Vector4d(1.0, t, t * t, t * t * t);
double minDistance = max(-m_viewFrustum.nearZ(), abs(p.z()) - segmentBoundingRadius);
if (segmentBoundingRadius >= m_subdivisionThreshold * minDistance)
{
if (m_viewFrustum.cullSphere(p, segmentBoundingRadius))
{
if (!restartCurve)
{
m_vbuf.end();
restartCurve = true;
}
}
else
{
restartCurve = renderCubic(restartCurve,
coeff, t - dt, t,
segmentBoundingRadius,
depth + 1);
}
}
else
{
#if DEBUG_ADAPTIVE_SPLINE
{
int c = depth % 10;
glColor4f(SplineColors[c][0], SplineColors[c][1], SplineColors[c][2], i % 2 ? 0.25f : 1.0f);
}
#endif
if (restartCurve)
{
m_vbuf.begin();
m_vbuf.vertex(lastP);
restartCurve = false;
}
m_vbuf.vertex(p);
}
lastP = p;
}
return restartCurve;
}
