unsigned IniGrainCenters(CONFIG& config)
{
Vector3d a;
Vector3d rotv;
Vector3d rtmp;
double angle;
time_t time1, time2;
time(&time1);
cout << "Initialize grain centers ";
for (int ig=0; ig<config.grains; ig++)
{
for (int dim=0; dim<3; dim++)
{
config.grain[ig].r(dim) = config.shift(dim) * (rand()%2000000/1000000. - 1);
}
for (unsigned j=0; j<3; j++)
{
config.grain[ig].angle(j) = 2.0 * M_PI * (rand()%1000000/1000000.);
}
a = config.grain[ig].angle;
rotv << 1/cos(a(0))*sin(a(1)), 1/sin(a(0))*sin(a(1)), 1/cos(a(1));
angle = -a(2);
rtmp = -config.grain[ig].r;
config.grain[ig].rotvT = rtmp * cos(angle) + rotv.cross(rtmp) * sin(angle) + (1 - cos(angle)) * rotv *(rotv.dot(rtmp)) + config.grain[ig].r; // Rodrigue's rotation formula
cout << ".";
}
time(&time2);
if (config.time) cout << endl << "Done in " << time2-time1 << " s.";
cout << endl;
return 0;
}
boost::optional<Contact>
NarrowPhaseCollisionDetector::xeno_collide(PhysicsObject& objectA, PhysicsObject& objectB,
const SupportMapping_CPtr& mapping, const SupportMapping_CPtr& mappingA,
const SupportMapping_CPtr& mappingB, const Vector3d& v0,
const Vector3d& relativeMovement) const
try
{
//~~~~~~~~~~~~~~~~~~~~~~~~~~
// Phase 1: Portal Discovery
//~~~~~~~~~~~~~~~~~~~~~~~~~~
Vector3d v1, v2, v3;
// Find the support point in the direction of the origin ray 0 - v0 = -v0.
Vector3d n = (-v0).normalize(); // note: v0 is guaranteed to be non-zero by construction
v1 = (*mapping)(n);
if(n.dot(v1) <= 0)
{
// Special Case: The origin doesn't lie on the near side of the support plane containing v1 -> MISS.
// (Note that the plane equation is given by n.x - n.v1 = 0, so if n.v1 <= 0 then n.x - n.v1 >= 0,
// i.e. the origin is in front of, or on, a support plane pointing away from the shape.)
return boost::none;
}
// Find the support point along the normal of the plane containing the origin, v0 and v1.
n = v0.cross(v1);
if(n.length_squared() < EPSILON*EPSILON)
{
// Special Case: v1 lies on the line joining the origin and v0.
// Since it's a point on the surface of the Minkowski difference shape,
// we can use it to test directly whether or not the origin is within
// the shape.
Vector3d v01 = v1 - v0;
if(v0.length_squared() < v01.length_squared())
{
// v0 is closer to the origin than it is to v1, so (by the geometry of the situation) the origin
// must lie between v0 and v1 and thus be inside the Minkowski difference shape -> HIT.
return make_contact(v0, mappingA, mappingB, relativeMovement, objectA, objectB);
}
else return boost::none;
}
n.normalize(); // note: if we get here, n is non-zero
v2 = (*mapping)(n);
if(n.dot(v2) <= 0)
{
// Special Case: The origin doesn't lie on the near side of the support plane containing v2 -> MISS.
return boost::none;
}
// Calculate the normal for the plane containing v0, v1 and v2.
n = (v1 - v0).cross(v2 - v0);
// We know that v1 != v0 and v2 != v0 by assumption that v0 is an interior point of the shape
// (note that v1 and v2 are both support points, and thus on the surface by definition). The
// only remaining way n could be 0 is if v1 - v0 and v2 - v0 were in the same direction. In
// that case, v2 would lie in the same plane as the origin, v0 and v1, since v1 - v0 does. But
// v2 is the support point in a direction perpendicular to this plane, so that could only happen
// if the plane itself was on the surface. But then v0 would be on the surface -> contradiction.
n.normalize();
// Ensure the origin is in front of the plane (which has equation n.x - n.v0 = 0)
// by swapping v1 and v2 if necessary. Note that to plug the origin into the equation
// we test n.0 - n.v0 = -n.v0 against 0. If it's less than 0, the origin's behind the
// plane. (This is the same as checking for n.v0 > 0.)
if(n.dot(v0) > 0)
{
std::swap(v1, v2);
n = -n;
}
// Iteratively find the remaining support point we need. Initially we look along the normal of the
// plane containing triangle (v0, v1, v2).
const int ITERATION_LIMIT = INT_MAX; // we limit the amount of work the collision detector is allowed to do
int iterations = 0;
while(iterations++ < ITERATION_LIMIT)
{
// Find the support point in the direction normal to the existing plane.
v3 = (*mapping)(n);
if(n.dot(v3) <= 0)
{
// Special Case: The origin doesn't lie on the near side of the support plane containing v3 -> MISS.
return boost::none;
}
// Check the origin against the the two other non-portal planes - if it's not in front of both of them,
// we need to keep looking for support points to make up our initial portal.
// Check against the plane containing triangle (v0, v2, v3). This plane has equation
// n.x - n.v0 = 0, where n = (v2-v0) x (v3 - v0). To test if the origin is behind it,
// we therefore check -n.v0 < 0, i.e. n.v0 > 0. This is v0 . ((v2 - v0) x (v3 - v0)) > 0.
// But note the identity a.((b-a) x (c-a)) = a.b x c, so this simplifies to v0.v2 x v3 > 0.
if(v0.dot(v2.cross(v3)) > 0)
{
// The origin is behind the plane containing (v0, v2, v3), so set things up
// to search for the third support point in front of that plane. In particular,
// we want to set (v0, v1, v2) := (v0, v3, v2), i.e. the reverse of the plane
// the origin's behind.
v1 = v3;
n = (v1 - v0).cross(v2 - v0).normalize(); // note that our plane is now that of triangle (v0, v1, v2)
continue;
}
// Check against the plane containing triangle (v0, v3, v1).
if(v0.dot(v3.cross(v1)) > 0)
{
// The origin is behind the plane containing (v0, v3, v1), so set things up
// to search for the third support point in front of that plane. In particular,
// we want to set (v0, v1, v2) := (v0, v1, v3), i.e. the reverse of the plane
// the origin's behind.
v2 = v3;
n = (v1 - v0).cross(v2 - v0).normalize(); // note that our plane is now that of triangle (v0, v1, v2)
continue;
}
// If we got here, we found a valid portal.
break;
}
//~~~~~~~~~~~~~~~~~~~~~~~~~~~
// Phase 2: Portal Refinement
//~~~~~~~~~~~~~~~~~~~~~~~~~~~
while(iterations++ < ITERATION_LIMIT)
{
// Check whether the origin's behind the portal (v1, v2, v3), which faces towards the outside of the shape.
// If so, it's inside the shape -> HIT. The plane is n.x - n.v1 = 0, where n = (v2-v1) x (v3-v1). The origin
// is behind it if -n.v1 < 0, i.e. if n.v1 > 0.
n = (v2 - v1).cross(v3 - v1).normalize();
if(n.dot(v1) > 0)
{
return make_contact(v0, mappingA, mappingB, relativeMovement, objectA, objectB);
}
// If the origin's not behind the portal, find the support plane in the direction of the portal normal.
// This has equation n.x - n.v4 = 0.
Vector3d v4 = (*mapping)(n);
// Check whether the origin's not behind the support plane, which faces towards the outside of the shape.
// If so, it's outside the shape -> MISS. The origin's not behind the support plane if -n.v4 >= 0, i.e.
// if n.v4 <= 0.
if(n.dot(v4) <= 0)
{
return boost::none;
}
// Check whether the support plane's sufficiently close to the portal that the origin might as well be
// outside the shape. If so -> MISS. The distance between the two planes is n.v4 - n.v1 = n.(v4 - v1).
double supportPlaneOffset = n.dot(v4 - v1);
if(supportPlaneOffset < EPSILON)
{
return boost::none;
}
// Find a new portal which is closer to the surface of the shape than the old one.
// This involves constructing a tetrahedron from the support point and the existing
// portal, then working out which of the outer faces of the tetrahedron the origin
// ray passes through - this outer face becomes the new portal. To do this, we look
// at the dividing planes containing (v0,vi,v4) for i in {1,2,3} and work out which
// segment of the tetrahedron the origin is in by testing it against them. Note that
// the displacement value obtained by plugging the origin into dividing plane i can
// be calculated as -v0.((vi-v0) x (v4-v0)) = -v0.vi x v4.
double d1 = -v0.dot(v1.cross(v4));
double d2 = -v0.dot(v2.cross(v4));
double d3 = -v0.dot(v3.cross(v4));
if(d1 < 0) // the origin's behind dividing plane 1
{
if(d2 < 0) // the origin's behind dividing plane 2
{
v1 = v4;
}
else // the origin's in front of dividing plane 2
{
v3 = v4;
}
}
else // the origin's in front of dividing plane 1
{
if(d3 < 0) // the origin's behind dividing plane 3
{
v2 = v4;
}
else // the origin's in front of dividing plane 3
{
v1 = v4;
}
}
}
// If we get here, we must have run out of iterations, so just return a MISS.
return boost::none;
}
catch(Exception&)
{
// None of the normalizations should fail, but just in case they do, it's better to miss a collision
// than risk crashing the entire program.
return boost::none;
}
/// Determines the barycentric coordinates corresponding to a point for this triangle
void Triangle::determine_barycentric_coords(const Point3d& p, double& s, double& t) const
{
const unsigned X = 0, Y = 1, Z = 2;
const Vector3d v0 = b - a, v1 = c - a, v2 = p - a;
double d00 = v0.dot(v0);
double d01 = v0.dot(v1);
double d11 = v1.dot(v1);
double d20 = v2.dot(v0);
double d21 = v2.dot(v1);
double denom = d00 * d11 - d01 * d01;
double v = (d11 * d20 - d01 * d21)/denom;
double w = (d00 * d21 - d01 * d20)/denom;
double u = 1.0 - v - w;
s = u;
t = v;
assert((a*s + b*t + c*(1.0-s-t) - p).norm() < NEAR_ZERO);
/*
// determine which coordinate has the minimum variance
std::pair<Point3d, Point3d> aabb = calc_AABB();
const double varx = aabb.second[X] - aabb.first[X];
const double vary = aabb.second[Y] - aabb.first[Y];
const double varz = aabb.second[Z] - aabb.first[Z];
// project to 2D
Point2d tri_2D[3];
Point2d v_2D;
if (varx < vary && varx < varz)
{
// minimum variance in the x direction; project to yz plane
tri_2D[0] = Point2d(a[Y], a[Z]);
tri_2D[1] = Point2d(b[Y], b[Z]);
tri_2D[2] = Point2d(c[Y], c[Z]);
v_2D = Point2d(v[Y], v[Z]);
}
else if (vary < varx && vary < varz)
{
// minimum variance in the y direction; project to xz plane
tri_2D[0] = Point2d(a[X], a[Z]);
tri_2D[1] = Point2d(b[X], b[Z]);
tri_2D[2] = Point2d(c[X], c[Z]);
v_2D = Point2d(v[X], v[Z]);
}
else
{
// minimum variance in the z direction; project to xy plane
tri_2D[0] = Point2d(a[X], a[Y]);
tri_2D[1] = Point2d(b[X], b[Y]);
tri_2D[2] = Point2d(c[X], c[Y]);
v_2D = Point2d(v[X], v[Y]);
}
// now determine barycentric coordinates for 2D triangle
determine_barycentric_coords(tri_2D, v_2D, s, t);
/*
/*
// minimum variance is in x direction
if (varx < vary && varx < varz)
{
const double r1 = v[Y] - _c[Y];
const double r2 = v[Z] - _c[Z];
const double y1 = _a[Y];
const double y2 = _b[Y];
const double y3 = _c[Y];
const double z1 = _a[Z];
const double z2 = _b[Z];
const double z3 = _c[Z];
const double denom = 1.0/(-y2*z1 + y3*z1 + y1*z2 - y3*z2 - y1*z3 + y2*z3);
double rs1 = r1*denom;
double rs2 = r2*denom;
s = (z2-z3)*rs1 + (y3-y2)*rs2;
t = (z3-z1)*rs1 + (y1-y3)*rs2;
}
// minimum variance is in y direction
else if (vary < varx && vary < varz)
{
const double r1 = v[X] - _c[X];
const double r2 = v[Z] - _c[Z];
const double x1 = _a[X];
const double x2 = _b[X];
const double x3 = _c[X];
const double z1 = _a[Z];
const double z2 = _b[Z];
const double z3 = _c[Z];
const double denom = 1.0/(-x2*z1 + x3*z1 + x1*z2 - x3*z2 - x1*z3 + x2*z3);
double rs1 = r1*denom;
double rs2 = r2*denom;
s = (z2-z3)*rs1 + (x3-x2)*rs2;
t = (z3-z1)*rs1 + (x1-x3)*rs2;
}
// minimum variance is in z direction
else
{
const double r1 = v[X] - _c[X];
const double r2 = v[Y] - _c[Y];
const double x1 = _a[X];
const double x2 = _b[X];
const double x3 = _c[X];
const double y1 = _a[Y];
const double y2 = _b[Y];
const double y3 = _c[Y];
const double denom = 1.0/(-x2*y1 + x3*y1 + x1*y2 - x3*y2 - x1*y3 + x2*y3);
double rs1 = r1*denom;
double rs2 = r2*denom;
s = (y2-y3)*rs1 + (x3-x2)*rs2;
t = (y3-y1)*rs1 + (x1-x3)*rs2;
}
*/
}
int
main (int argc, char **argv)
{
// Load sphere surface:
GmshSurfaceLoader surface_loader;
shared_ptr<Surface> sphere(new Surface("main"));
surface_loader.load(sphere, string("sphere.msh"));
// Create surface body:
shared_ptr<Body> body(new Body(string("sphere")));
body->add_non_lifting_surface(sphere);
// Set up solver:
Solver solver("test-sphere-log");
solver.add_body(body);
Vector3d freestream_velocity(30.0, 0, 0);
solver.set_freestream_velocity(freestream_velocity);
double fluid_density = 1.2;
solver.set_fluid_density(fluid_density);
// Run simulation:
solver.solve();
// Check pressure coefficients and surface potential values:
for (int i = 0; i < sphere->n_panels(); i++) {
Vector3d x = sphere->panel_collocation_point(i, false);
// Angle between flow and point on sphere:
double theta = acos(x.dot(freestream_velocity) / (x.norm() * freestream_velocity.norm()));
// Analytical solution for pressure coefficient and surface potential.
// See J. Katz and A. Plotkin, Low-Speed Aerodynamics, Cambridge University Press, 2001.
double C_p_ref = 1.0 - 9.0 / 4.0 * pow(sin(theta), 2);
double R = x.norm();
double phi_ref = freestream_velocity.norm() * cos(theta) * (R + pow(R, 3) / (2 * pow(R, 2)));
// Computed pressure coefficient:
double C_p = solver.pressure_coefficient(sphere, i);
double phi = solver.surface_velocity_potential(sphere, i);
// Compare:
if (fabs(C_p - C_p_ref) > TEST_TOLERANCE) {
cerr << " *** TEST FAILED *** " << endl;
cerr << " theta = " << theta << " rad" << endl;
cerr << " C_p(ref) = " << C_p_ref << endl;
cerr << " C_p = " << C_p << endl;
cerr << " ******************* " << endl;
exit(1);
}
if (fabs(phi - phi_ref) / (R * freestream_velocity.norm()) > TEST_TOLERANCE) {
cerr << " *** TEST FAILED *** " << endl;
cerr << " theta = " << theta << " rad" << endl;
cerr << " phi(ref) = " << phi_ref << endl;
cerr << " phi = " << phi << endl;
cerr << " ******************* " << endl;
exit(1);
}
}
// Done:
return 0;
}
Vector3d traceRay(Ray* ray, vector<SceneObject*>* objects, vector<SceneObject*>* lights, int remainingDepth) {
Vector3d backgroundColour(0, 0, 0);
Vector3d ambientLight(25, 25, 25);
/*Vector3d RED(255, 0, 0);
Vector3d GREEN(0, 255, 0);
Vector3d BLUE(255, 0, 255);
Vector3d YELLOW(255, 255, 0);
Vector3d CYAN(0, 255, 255);
Vector3d MAJENTA(255, 0, 255);*/
if (remainingDepth <= 0)
return backgroundColour;
vector<Intersection*>* intersections = getIntersections(objects, ray, NULL, false, (remainingDepth < 2));
Intersection* closestIntersection = getClosestIntersection(ray->origin, intersections);
Vector3d closestOrigin;
Vector3d closestDirection;
SceneObject* closestObject;
if (closestIntersection != NULL) {
closestOrigin = *(closestIntersection->origin);
closestDirection = *(closestIntersection->direction);
closestObject = closestIntersection->object;
}
freeIntersections(intersections);
intersections->clear();
delete intersections;
if (closestIntersection != NULL) {
Vector3d fullLightColour = ambientLight;
Vector3d surfaceColour = *(closestIntersection->object->colour);
//for each light, add it to the full light on this point (if not blocked)
for (unsigned int lightNum = 0; lightNum < lights->size(); lightNum++) {
SceneObject* light = (*lights)[lightNum];
Vector3d toLight = *(light->position) - closestOrigin;
Vector3d toLightNormalized = toLight.normalized();
double dot = toLightNormalized.dot(closestDirection);
if (dot > 0) {
intersections = getIntersections(objects, new Ray(&closestOrigin, &toLightNormalized), closestObject, true, false);
bool inLight = false;
if (intersections->size() == 0) {
inLight = true;
}
else {
Intersection* intersection = (*intersections)[0];
Vector3d intersectionOrigin = Vector3d(*(intersection->origin));
Vector3d intersectionDirection = Vector3d(*(intersection->direction));
SceneObject* obj = intersection->object;
if ((intersectionOrigin - closestOrigin).norm() > toLight.norm()) {
inLight = true;
}
}
if (inLight) {
fullLightColour += *(light->colour) * dot;
}
freeIntersections(intersections);
intersections->clear();
delete intersections;
}
}
double reflectivity = closestObject->reflectivity;
if (reflectivity > 0) {
Vector3d rayDirection = *(ray->direction);
Vector3d normal = closestDirection;
Vector3d reflectedDirection = rayDirection - ((2 * (normal.dot(rayDirection))) * normal);
Ray* reflectedRay = new Ray(&closestOrigin, &reflectedDirection);
Vector3d reflectionColour = traceRay(reflectedRay, objects, lights, remainingDepth - 1);
delete reflectedRay;
surfaceColour *= 1 - reflectivity;
surfaceColour += reflectionColour * reflectivity;
}
Vector3d endColour = surfaceColour;
endColour[0] *= fullLightColour[0] / 255;
endColour[1] *= fullLightColour[1] / 255;
endColour[2] *= fullLightColour[2] / 255;
return endColour;
}
else {
return backgroundColour;
}
}
VectorXd rigidBodyDynamics::f(VectorXd x) {
Vector3d dr, dv, da, dw;
Matrix<double,12,12> lambda, dLambda;
VectorXd vec_dLambda;
int vecsize;
if (covProp) {
vecsize = 90;
} else {
vecsize = 12;
}
VectorXd dx(vecsize);
Vector3d r = x.segment<3>(0);
Vector3d v = x.segment<3>(3);
Vector3d a = x.segment<3>(6);
Vector3d w = x.segment<3>(9);
MatrixXd Bw = getBw();
Matrix3d J = _ir.getJ();
//Nonlinear State Model \dot x = f(x)
/*
* \mathbf{\dot r} = \mathbf{v}
*/
dr = v;
/*
* \mathbf{\dot v} = 0
*/
dv = Vector3d::Zero();
/*
* \frac{d \mathbf{a}_p}{dt} =
* \frac{1}{2}\left(\mathbf{[\omega \times]} +
* \mathbf{\omega} \cdot \mathbf{\bar q} \right) \mathbf{a}_p +
* \frac{2 q_4}{1+q_4} \mathbf{\omega}
*/
double c1, c2, c3;
c1 = 0.5;
c2 = 0.125 * w.dot(a); //NEW simplification
c3 = 1 - a.dot(a)/16;
da = -c1 * w.cross(a) + c2* a + c3 * w;
/*
* \dot \mathbf{w} = -\mathbf{J}^{-1} \mathbf{\omega} \times \mathbf{J} \mathbf{\omega}
*/
dw = - J.inverse() * w.cross(J * w);
if (covProp) {
//Covariance Propagation according to Lyapunov function
//see Brown & Hwang pg 204
//Compute Linear transition matrix
Matrix<double,12,12> A = Matrix<double,12,12>::Zero();
//position derivative
A.block<3,3>(0,3) = Matrix<double,3,3>::Identity();
//mrp kinematics
A.block<3,3>(6,6) = -0.5*crossProductMat(w) + w.dot(a)/8 * Matrix3d::Identity();
A.block<3,3>(6,9) = (1-a.dot(a/16))*Matrix3d::Identity();
//angular velocity dynamics
A.block<3,3>(9,9) = - J.inverse() * crossProductMat(w) * J;
lambda = vec2symmMat(x.segment<78>(12));
dLambda = A * lambda + lambda *A.transpose() + Bw * _Q * Bw.transpose();
vec_dLambda = symmMat2Vec(dLambda);
}
//write to dx
dx.segment<3>(0) = dr;
dx.segment<3>(3) = dv;
dx.segment<3>(6) = da;
dx.segment<3>(9) = dw;
if(covProp){
dx.segment<78>(12) = vec_dLambda;
}
return dx;
}
// dist3D_Segment_to_Segment():
// Input: two 3D line segments S1 and S2
// Return: the shortest distance between S1 and S2
double dist3D_Segment_to_Segment(const Vector3d &S1P0, const Vector3d &S1P1,
const Vector3d &S2P0, const Vector3d &S2P1, double SMALL_NUM)
{
Vector3d u = S1P1 - S1P0;
Vector3d v = S2P1 - S2P0;
Vector3d w = S1P0 - S2P0;
double a = u.dot(u); // always >= 0
double b = u.dot(v);
double c = v.dot(v); // always >= 0
double d = u.dot(w);
double e = v.dot(w);
double D = a*c - b*b; // always >= 0
double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < SMALL_NUM) { // the lines are almost parallel
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else {
sN = -d;
sD = a;
}
}
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (abs(sN) < SMALL_NUM ? 0.0 : sN / sD);
tc = (abs(tN) < SMALL_NUM ? 0.0 : tN / tD);
// get the difference of the two closest points
Vector3d dP = w + (u * sc ) - (v * tc); // = S1(sc) - S2(tc)
return dP.length(); // return the closest distance
}
double vectorAngle(const Vector3d &r1,const Vector3d &r2){
return(acos(r1.dot(r2)/r1.norm()/r2.norm()));
}
int main(int argc, const char* argv[]){
if (argc<2)
{
cout << endl;
cout << "Please type: " << endl;
cout << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] [STRUCTURE_ONLY] [DENSE]" << endl;
cout << endl;
cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
cout << "OUTLIER_RATIO: probability of spuroius observation (default: 0.0)" << endl;
cout << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)" << endl;
cout << "STRUCTURE_ONLY: performe structure-only BA to get better point initializations (0 or 1; default: 0==false)" << endl;
cout << "DENSE: Use dense solver (0 or 1; default: 0==false)" << endl;
cout << endl;
cout << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to 1==true." << endl;
cout << endl;
exit(0);
}
double PIXEL_NOISE = atof(argv[1]);
double OUTLIER_RATIO = 0.0;
if (argc>2) {
OUTLIER_RATIO = atof(argv[2]);
}
bool ROBUST_KERNEL = false;
if (argc>3){
ROBUST_KERNEL = atoi(argv[3]) != 0;
}
bool STRUCTURE_ONLY = false;
if (argc>4){
STRUCTURE_ONLY = atoi(argv[4]) != 0;
}
bool DENSE = false;
if (argc>5){
DENSE = atoi(argv[5]) != 0;
}
cout << "PIXEL_NOISE: " << PIXEL_NOISE << endl;
cout << "OUTLIER_RATIO: " << OUTLIER_RATIO<< endl;
cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl;
cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl;
cout << "DENSE: "<< DENSE << endl;
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
if (DENSE) {
linearSolver= new g2o::LinearSolverDense<g2o
::BlockSolver_6_3::PoseMatrixType>();
} else {
linearSolver
= new g2o::LinearSolverCholmod<g2o
::BlockSolver_6_3::PoseMatrixType>();
}
g2o::BlockSolver_6_3 * solver_ptr
= new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
vector<Vector3d> true_points;
for (size_t i=0;i<500; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+3));
}
double focal_length= 1000.;
Vector2d principal_point(320., 240.);
vector<g2o::SE3Quat,
aligned_allocator<g2o::SE3Quat> > true_poses;
g2o::CameraParameters * cam_params
= new g2o::CameraParameters (focal_length, principal_point, 0.);
cam_params->setId(0);
if (!optimizer.addParameter(cam_params)) {
assert(false);
}
int vertex_id = 0;
for (size_t i=0; i<15; ++i) {
Vector3d trans(i*0.04-1.,0,0);
Eigen:: Quaterniond q;
q.setIdentity();
g2o::SE3Quat pose(q,trans);
g2o::VertexSE3Expmap * v_se3
= new g2o::VertexSE3Expmap();
v_se3->setId(vertex_id);
if (i<2){
v_se3->setFixed(true);
}
v_se3->setEstimate(pose);
optimizer.addVertex(v_se3);
true_poses.push_back(pose);
vertex_id++;
}
int point_id=vertex_id;
int point_num = 0;
double sum_diff2 = 0;
cout << endl;
tr1::unordered_map<int,int> pointid_2_trueid;
tr1::unordered_set<int> inliers;
for (size_t i=0; i<true_points.size(); ++i){
g2o::VertexSBAPointXYZ * v_p
= new g2o::VertexSBAPointXYZ();
v_p->setId(point_id);
v_p->setMarginalized(true);
v_p->setEstimate(true_points.at(i)
+ Vector3d(Sample::gaussian(1),
Sample::gaussian(1),
Sample::gaussian(1)));
int num_obs = 0;
for (size_t j=0; j<true_poses.size(); ++j){
Vector2d z = cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480){
++num_obs;
}
}
if (num_obs>=2){
optimizer.addVertex(v_p);
bool inlier = true;
for (size_t j=0; j<true_poses.size(); ++j){
Vector2d z
= cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480){
double sam = Sample::uniform();
if (sam<OUTLIER_RATIO){
z = Vector2d(Sample::uniform(0,640),
Sample::uniform(0,480));
inlier= false;
}
z += Vector2d(Sample::gaussian(PIXEL_NOISE),
Sample::gaussian(PIXEL_NOISE));
g2o::EdgeProjectXYZ2UV * e
= new g2o::EdgeProjectXYZ2UV();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertices().find(j)->second));
e->setMeasurement(z);
e->information() = Matrix2d::Identity();
if (ROBUST_KERNEL) {
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
}
e->setParameterId(0, 0);
optimizer.addEdge(e);
}
}
if (inlier){
inliers.insert(point_id);
Vector3d diff = v_p->estimate() - true_points[i];
sum_diff2 += diff.dot(diff);
}
pointid_2_trueid.insert(make_pair(point_id,i));
++point_id;
++point_num;
}
}
cout << endl;
optimizer.initializeOptimization();
optimizer.setVerbose(true);
if (STRUCTURE_ONLY){
g2o::StructureOnlySolver<3> structure_only_ba;
cout << "Performing structure-only BA:" << endl;
g2o::OptimizableGraph::VertexContainer points;
for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
if (v->dimension() == 3)
points.push_back(v);
}
structure_only_ba.calc(points, 10);
}
cout << endl;
cout << "Performing full BA:" << endl;
optimizer.optimize(10);
cout << endl;
cout << "Point error before optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
point_num = 0;
sum_diff2 = 0;
for (tr1::unordered_map<int,int>::iterator it=pointid_2_trueid.begin();
it!=pointid_2_trueid.end(); ++it){
g2o::HyperGraph::VertexIDMap::iterator v_it
= optimizer.vertices().find(it->first);
if (v_it==optimizer.vertices().end()){
cerr << "Vertex " << it->first << " not in graph!" << endl;
exit(-1);
}
g2o::VertexSBAPointXYZ * v_p
= dynamic_cast< g2o::VertexSBAPointXYZ * > (v_it->second);
if (v_p==0){
cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
exit(-1);
}
Vector3d diff = v_p->estimate()-true_points[it->second];
if (inliers.find(it->first)==inliers.end())
continue;
sum_diff2 += diff.dot(diff);
++point_num;
}
cout << "Point error after optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
cout << endl;
}
void robot_graphics_t::renderJoints(BodyNode *_node, RenderInterface *_ri, int _depth) {
if(!_node)
return;
// render self geometry
Joint *_jointParent = _node->getParentJoint();
int nt = _jointParent->getNumTransforms();
if(_depth > 0) {
// lines?
glColor3d(1,1,1);
glLineWidth(2.0);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3d(0, 0, 0);
Matrix4d locTrans = _jointParent->getLocalTransform();
// Lines?
glVertex3d(locTrans(0,3), locTrans(1,3), locTrans(2,3));
glEnd();
glEnable(GL_LIGHTING);
}
_ri->pushMatrix();
for(int i=0; i < _jointParent->getNumTransforms(); ++i) {
_jointParent->getTransform(i)->applyGLTransform(_ri);
// if(i == _jointParent->getNumTransforms()-1) {
// // dof transform
// cout << "BodyNode: " << _node->getName() << endl;
// cout << "Trfm: " << _jointParent->getTransform(i)->getName() << endl;
// cout << "Tfrm Type: " << _jointParent->getTransform(i)->getType() << endl;
// cout << "= \n" << _jointParent->getTransform(i)->getTransform() << endl;
// cout << "ParentJoint: " << _jointParent->getName() << endl;
// for(int i=0; i < 3; i++) {
// cout << "axis " << i << "= \n"
// << _jointParent->getAxis(i) << endl;
// }
// }
}
// axis 0 is joint axis of rotation (i think)
Vector3d ax = _jointParent->getAxis(0).normalized();
Vector3d zx = Vector3d::UnitZ();
Vector3d perp = zx.cross(ax);
double y = perp.norm();
double x = zx.dot(ax);
double ang = atan2(y,x) * 180.0 / M_PI;
_ri->pushMatrix();
double radius = 0.02;
double height = 0.05;
_ri->rotate(perp, ang);
_ri->pushMatrix();
glTranslated(0.0,0.0,-height/2);
glColor3d(0.0, 0.3, 1.0);
QUAD_OBJ_INIT;
gluCylinder(quadObj, radius, radius, height, 8, 8);
gluDisk(quadObj, 0, radius, 8, 8);
glTranslated(0.0,0.0,height);
gluDisk(quadObj, 0, radius, 8, 8);
_ri->popMatrix();
//glColor3d(0.0, 1.0, 0.0);
//_ri->drawCylinder(radius, height);
_ri->popMatrix();
// render subtree
for(int i=0; i < _node->getNumChildJoints(); ++i) {
BodyNode *child = _node->getChildJoint(i)->getChildNode();
renderJoints(child, _ri, _depth+1);
}
_ri->popMatrix();
}
// Get the closest points in the direction of estimated movement using a triangle
std::vector<BlockInfo> ICP::closestPoints(intensityCloud::Ptr P, MLSM *X,std::vector<double> timeStamps, Vector3d eV){
std::vector<BlockInfo> result;
BlockInfo blockInfo;
mlsm::Block* blockPtr, *bestBlockPtr;
//intensityCloud::iterator cloudIterator = P->begin();
cell::iterator cellIterator;
int i, j;
int ci, cj;
cell cellPtr;
bool found = false;
double minError = DBL_MAX, d0;
Vector3d unitSpeed;
Vector3d unitDir;
double angle, closestAngle = M_PI;
//ROS_INFO("Vel %f, %f %f", eV[0], eV[1],eV[2]);
// if ((eV[0] == 0.0) && (eV[1] == 0.0)){
//ROS_INFO("No estimation");
pcl::PointXYZI p0,p[nSamples_];
double movingTime = 0;
double firstStamp = *(timeStamps.begin());
for(int i=0;i<P->size();i++){
if (isnan(P->at(i).x) || isnan(P->at(i).y) || isnan(P->at(i).z)) continue;
movingTime = fabs(timeStamps[i]) - fabs(timeStamps.front());//lastStamp - timeStamps[i];
closestAngle = M_PI;
p0 = P->at(i);
//ROS_INFO("Point %d X %f Y %f Z %f",i, p0.x, p0.y, p0.z);
bestBlockPtr = X->findClosestBlock(p0);
//ROS_INFO("Candt %d X %f Y %f Z %f\n",i,bestBlockPtr->mean_.x,bestBlockPtr->mean_.y,bestBlockPtr->mean_.z);
//ROS_INFO("ICP: Nsamples is %d", nSamples_);
if ((((eV[0] != 0) || (eV[1] != 0) || (eV[2] != 0)) || (timeStamps.size() != P->size())) && (nSamples_ > 0)){
unitSpeed[0] = eV[0];
unitSpeed[1] = eV[1];
unitSpeed[2] = eV[2];
unitSpeed.normalize();
for (int j=0;j<nSamples_;j++){
p[j].x = p0.x + movingTime* eV[0] * j * sampleStep_;
p[j].y = p0.y + movingTime* eV[1] * j * sampleStep_;
p[j].z = p0.z + movingTime* eV[2] * j * sampleStep_;
//ROS_INFO("Point Sample %d X %f Y %f Z %f",j, p[j].x, p[j].y, p[j].z);
blockPtr = X->findClosestBlock(p[j]);
if (blockPtr == NULL) break;
// Find angle between the block and p0
unitDir[0] = blockPtr->mean_.x - p0.x;
unitDir[1] = blockPtr->mean_.y - p0.y;
unitDir[2] = blockPtr->mean_.z - p0.z;
unitDir.normalize();
angle = acos(unitSpeed.dot(unitDir));
if (fabs(angle) < closestAngle){
bestBlockPtr = blockPtr;
closestAngle = angle;
}
}
}
blockInfo.blockPtr = bestBlockPtr;
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
minError = DBL_MAX;
}
////ROS_INFO("Finished finding closest points");
return result;
/*Directional grid based search based on bresenham. No longer needed.
* }
//ROS_INFO("With estimation");
if (eV[0] > eV[1]){
eV[1] = eV[1] / eV[0];
eV[0] = 1;
}
else{
eV[0] = eV[0] / eV[1];
eV[1] = 1;
}
double m = 100.0;
int stepY = 1;
int stepX = 1;
int error;
int errorprev;
int incY;
int incX;
for (;cloudIterator != P->end();cloudIterator++){
if (isnan(cloudIterator->x) || isnan(cloudIterator->y) || isnan(cloudIterator->z)) continue;
////ROS_INFO("Point x = %f, resolution = %f", cloudIterator->x, X->getResolution());
i = cloudIterator->x / X->getResolution();
j = cloudIterator->y / X->getResolution();
////ROS_INFO("%d, %d", i, j);
incY = abs(m * (eV[1] - j));
incX = abs(m * (eV[0] - i));
stepY = 1;
stepX = 1;
if (incY < 0){
stepY = -1;
}
if (incX < 0){
stepX = -1;
}
if (incX >= incY){
error = incY - incX;
errorprev = error;
while ((i < X->getSpanX())&&(j < X->getSpanY())){
////ROS_INFO("%d, %d", i, j);
if (X->checkCell(i,j)){
////ROS_INFO("CHECK %d, %d", i, j);
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
if (error >= 0){
j += stepY;
errorprev = error;
error -= incX;
if (error + errorprev < incX){
if (X->checkCell(i,j - stepY)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
}
if (error + errorprev > incX){
if (X->checkCell(i - stepX,j)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
}
else {
if (X->checkCell(i,j - stepY)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
if (X->checkCell(i - stepX,j)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
}
}
i += stepX;
error += incY;
}
}
else {
error = incX - incY;
errorprev = error;
while ((i < X->getSpanX())&&(j < X->getSpanY())){
if (X->checkCell(i,j)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
if (error >= 0){
i += stepX;
errorprev = error;
error -= incY;
if (error + errorprev < incY){
if (X->checkCell(i - stepX,j)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
}
if (error + errorprev > incY){
if (X->checkCell(i,j - stepY)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
}
else {
if (X->checkCell(i,j - stepY)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
if (X->checkCell(i - stepX,j)){
blockInfo.blockPtr = X->findSuitableBlock(i,j,*cloudIterator);
blockInfo.i = i;
blockInfo.j = j;
result.push_back(blockInfo);
break;
}
}
}
j += stepY;
error += incX;
}
}
}
return result;*/
}
void ThreadPiece::calculateBinormal_withLength(const Vector3d& edge_prev, const Vector3d& edge_after, Vector3d& binormal)
{
binormal = 2.0*edge_prev.cross(edge_after);
binormal /= (edge_prev.norm()*edge_after.norm() + edge_prev.dot(edge_after));
}
void ThreadPiece::calculateBinormal(const Vector3d& edge_prev, const Vector3d& edge_after, Vector3d& binormal)
{
binormal = 2.0*edge_prev.cross(edge_after);
binormal /= ((_my_thread->rest_length())*(_my_thread->rest_length()) + edge_prev.dot(edge_after));
}
unsigned RotateBox(CONFIG& config, int ig)
{
Vector3d a;
Vector3d rotv;
Vector3d rtmp;
double angle;
time_t time1, time2;
time(&time1);
a = config.grain[ig].angle;
rotv << cos(a(0))*sin(a(1)), sin(a(0))*sin(a(1)), cos(a(1));
angle = a(2);
for (int i = 0; i < config.atoms_grain; i++)
{
rtmp = config.atom_grain[i].r;
config.atom_grain[i].r = rtmp * cos(angle) + rotv.cross(rtmp) * sin(angle) + (1 - cos(angle)) * rotv *(rotv.dot(rtmp)) + config.grain[ig].r; // Rodrigue's rotation formula
}
time(&time2);
if (config.time) cout << "Rotated in " << time2-time1 << " s.";
return 0;
}
void Thread_RRT::findThreadMotion(RRT_Node& start, RRT_Node& goal, Thread_Motion& bestMotion)
{
/*motion.pos_movement.setZero();
motion.tan_rotation.setIdentity();
double estimated_derivs[5];
Matrix4d startTrans;
start.thread->getStartTransform(startTrans);
for (int dir=0; dir < 3; dir++)
{
//move in direction
motion.pos_movement(dir) = DISTANCE_TO_TRY;
RRT_Node thisNodeToTry(motion.applyMotion(start.thread));
double pos_dir_dist = goal.distanceToNode(thisNodeToTry);
//move in other direction
motion.pos_movement(dir) = -DISTANCE_TO_TRY;
RRT_Node thisNodeToTryOtherDir(motion.applyMotion(start.thread));
double neg_dir_dist = goal.distanceToNode(thisNodeToTryOtherDir);
//double neg_dir_dist = goal.distanceToNode(start);
estimated_derivs[dir] = (pos_dir_dist-neg_dir_dist)/(2.0*DISTANCE_TO_TRY);
//cleanup for next iter
motion.pos_movement(dir) = 0.0;
}
std::cout << estimated_derivs[0] << " " << estimated_derivs[1] << " " << estimated_derivs[2] << std::endl;
motion.pos_movement = Vector3d(estimated_derivs[0]/100.0, estimated_derivs[1]/100.0, estimated_derivs[2]/100.0);
*/
/*
Matrix4d startTrans;
start.thread->getStartTransform(startTrans);
double bestScore = DBL_MAX;
Thread_Motion thisMotion;
for (int randSampleInd=0; randSampleInd < NUM_RANDOM_SAMPLES; randSampleInd++)
{
//random position movement
for (int i=0; i < 3; i++)
{
thisMotion.pos_movement(i) = (2.0*MAX_DISTANCE_MOVE_EACH_DIR*((double)rand()) / ((double)RAND_MAX))-MAX_DISTANCE_MOVE_EACH_DIR;
}
//random rotation matrix
double rotAng1 = (2.0*MAX_ANG_TO_ROTATE*((double)rand()) / ((double)RAND_MAX)) - MAX_ANG_TO_ROTATE;
double rotAng2 = (2.0*MAX_ANG_TO_ROTATE*((double)rand()) / ((double)RAND_MAX)) - MAX_ANG_TO_ROTATE;
thisMotion.setRotationMatrixFromAngs(rotAng1, rotAng2);
//std::cout << "pos: " << thisMotion.pos_movement << std::endl;
//std::cout << "tan: " << thisMotion.tan_rotation << std::endl;
RRT_Node thisMotionTried(thisMotion.applyMotion(start.thread));
double thisScore = goal.distanceToNode(thisMotionTried);
if (thisScore < bestScore)
{
bestScore = thisScore;
bestMotion = thisMotion;
}
}
*/
//find vector to get position closer
Vector3d startEnd;
start.thread->getWantedEndPosition(startEnd);
Vector3d goalEnd;
goal.thread->getWantedEndPosition(goalEnd);
bestMotion.pos_movement = goalEnd - startEnd;
//add some noise to movement
bestMotion.pos_movement(0) += randomMaxAbsValue(MAX_NOISE_DISTANCE_MOVEMENT);
bestMotion.pos_movement(1) += randomMaxAbsValue(MAX_NOISE_DISTANCE_MOVEMENT);
bestMotion.pos_movement(2) += randomMaxAbsValue(MAX_NOISE_DISTANCE_MOVEMENT);
//make sure vector isn't too long (that's what she said)
double normOfVec = bestMotion.pos_movement.norm();
if (normOfVec > MAX_DISTANCE_MOVEMENT)
{
bestMotion.pos_movement *= (MAX_DISTANCE_MOVEMENT/normOfVec);
}
//rotate towards other tan
Vector3d startTan;
start.thread->getWantedEndTangent(startTan);
Vector3d goalTan;
goal.thread->getWantedEndTangent(goalTan);
Vector3d vecToRotateAbout = startTan.cross(goalTan);
vecToRotateAbout.normalize();
//add noise to tan
vecToRotateAbout(0) += randomMaxAbsValue(MAX_NOISE_ROTATION_VECTOR);
vecToRotateAbout(1) += randomMaxAbsValue(MAX_NOISE_ROTATION_VECTOR);
vecToRotateAbout(2) += randomMaxAbsValue(MAX_NOISE_ROTATION_VECTOR);
vecToRotateAbout.normalize();
double angToRotate = min(MAX_ANG_TO_ROTATE, acos(startTan.dot(goalTan)));
angToRotate += randomMaxAbsValue(MAX_NOISE_ROTATION_ANGLE);
bestMotion.tan_rotation = Eigen::AngleAxisd( angToRotate, vecToRotateAbout);
//motion.pos_movement = Vector3d(4.0, -3.5, 1.0);
//motion.tan_rotation = Eigen::AngleAxisd (0.0, Vector3d(1.0, 0.0, 0.0));
}
Vector3d gc_plucker_origin(Vector3d n, Vector3d v) {
return v.cross(n) / v.dot(v);
}
void make_vectors_perpendicular(const Vector3d& start, Vector3d& to_be_perp)
{
to_be_perp = -(to_be_perp - (to_be_perp.dot(start.normalized()))*start.normalized()).normalized();
}
double gc_angle_normvec( Vector3d v1, Vector3d v2 ) {
return acos( v1.dot(v2) );
}
// Does the same as capsuleCapsuleDistance except that b_end is automatically set to b_start. Should be improved.
double capsuleSphereDistance(const Vector3d& a_start, const Vector3d& a_end, const double a_radius, const Vector3d& b_start, const double b_radius, Vector3d& direction)
{
Vector3d b_end = b_start;
// Line parametrization
// L1 : P(s) = a_start + s * (a_end - a_start) = a_start + s * u
// L2 : Q(t) = b_start + t * (b_end - b_start) = b_start + t * v
Vector3d u = a_end - a_start;
Vector3d v = b_end - b_start;
Vector3d w = a_start - b_start;
Vector3d a_end_minus_b_start = a_end - b_start;
Vector3d a_start_minus_b_end = a_start - b_end;
double u_dot_u = u.dot(u);
double u_dot_v = u.dot(v);
double v_dot_v = v.dot(v);
double u_dot_w = u.dot(w);
double v_dot_w = v.dot(w);
// SPHERE-SPHERE collision
Vector3d start_sphere_start_sphere = w;
double start_sphere_start_sphere_squared_dist = start_sphere_start_sphere.squaredNorm();
Vector3d start_sphere_end_sphere = a_start_minus_b_end;
double start_sphere_end_sphere_squared_dist = start_sphere_end_sphere.squaredNorm();
Vector3d end_sphere_start_sphere = a_end_minus_b_start;
double end_sphere_start_sphere_squared_dist = end_sphere_start_sphere.squaredNorm();
Vector3d end_sphere_end_sphere = a_end - b_end;
double end_sphere_end_sphere_squared_dist = end_sphere_end_sphere.squaredNorm();
// SPHERE-CYLINDER collision
// Ignores the case when the center of the sphere is outside the range of the line segment even though the sphere might still collide with the cylinder's end cap.
// This case is taken care by sphere-sphere collision.
double t; // line parameter of the cylinder's axis
Vector3d start_sphere_cyl, end_sphere_cyl, cyl_start_sphere, cyl_end_sphere;
double start_sphere_cyl_squared_dist, end_sphere_cyl_squared_dist, cyl_start_sphere_squared_dist, cyl_end_sphere_squared_dist;
t = v_dot_w/v.squaredNorm();
if (t < 0 || t > 1) {
start_sphere_cyl_squared_dist = numeric_limits<double>::max();
} else {
start_sphere_cyl = (w - t*v);
start_sphere_cyl_squared_dist = start_sphere_cyl.squaredNorm();
}
t = a_end_minus_b_start.dot(v)/v.squaredNorm();
if (t < 0 || t > 1) {
end_sphere_cyl_squared_dist = numeric_limits<double>::max();
} else {
end_sphere_cyl = (a_end_minus_b_start - t*v);
end_sphere_cyl_squared_dist = end_sphere_cyl.squaredNorm();
}
t = -u_dot_w/u.squaredNorm();
if (t < 0 || t > 1) {
cyl_start_sphere_squared_dist = numeric_limits<double>::max();
} else {
cyl_start_sphere = (w + t*u);
cyl_start_sphere_squared_dist = cyl_start_sphere.squaredNorm();
}
t = -a_start_minus_b_end.dot(u)/u.squaredNorm();
if (t < 0 || t > 1) {
cyl_end_sphere_squared_dist = numeric_limits<double>::max();
} else {
cyl_end_sphere = (a_start_minus_b_end + t*u);
cyl_end_sphere_squared_dist = cyl_end_sphere.squaredNorm();
}
// CYLINDER-CYLINDER collision
// Ignores the case when the closest distance vector is not perpendicular to the line segment.
// This case is taken care by the other two cases.
double D = u_dot_u * v_dot_v - u_dot_v * u_dot_v; // denominator for s and t parameter
Vector3d cyl_cyl;
double cyl_cyl_squared_dist;
if (D < INTERSECTION_PARALLEL_CHECK_FACTOR) { // the lines are almost parallel
if (start_sphere_cyl_squared_dist == numeric_limits<double>::max() &&
end_sphere_cyl_squared_dist == numeric_limits<double>::max() &&
cyl_start_sphere_squared_dist == numeric_limits<double>::max() &&
cyl_end_sphere_squared_dist == numeric_limits<double>::max()) { // If the closest distance vector is not perpendicular to the line segments.
// In other words, if the closest distance of the infinite lines is not the same as the closest distance of the finite segments.
cyl_cyl_squared_dist = numeric_limits<double>::max();
} else {
cyl_cyl = (w - (v_dot_w/v_dot_v) * v);
cyl_cyl_squared_dist = cyl_cyl.squaredNorm();
}
} else { // the line segements are not almost parallel
double sN = (u_dot_v * v_dot_w - v_dot_v * u_dot_w); // nominator of s parameter
double tN = (u_dot_u * v_dot_w - u_dot_v * u_dot_w); // nominator of t parameter
if (sN < 0.0 || sN > D || tN < 0.0 || tN > D) { // If the closest distance vector is not perpendicular to the line segments.
// In other words, if the closest distance doesn't happen within the segment limits.
cyl_cyl_squared_dist = numeric_limits<double>::max();
} else {
cyl_cyl = (w + (sN/D * u) - (tN/D * v));
cyl_cyl_squared_dist = cyl_cyl.squaredNorm();
}
}
// MINIMUN OF THE SQUARED DISTANCES
double min_squared_dist = start_sphere_start_sphere_squared_dist;
direction = start_sphere_start_sphere;
if (min_squared_dist >= start_sphere_end_sphere_squared_dist) {
min_squared_dist = start_sphere_end_sphere_squared_dist;
direction = start_sphere_end_sphere;
}
if (min_squared_dist >= end_sphere_start_sphere_squared_dist) {
min_squared_dist = end_sphere_start_sphere_squared_dist;
direction = end_sphere_start_sphere;
}
if (min_squared_dist >= end_sphere_end_sphere_squared_dist) {
min_squared_dist = end_sphere_end_sphere_squared_dist;
direction = end_sphere_end_sphere;
}
if (min_squared_dist >= start_sphere_cyl_squared_dist) {
min_squared_dist = start_sphere_cyl_squared_dist;
direction = start_sphere_cyl;
}
if (min_squared_dist >= end_sphere_cyl_squared_dist) {
min_squared_dist = end_sphere_cyl_squared_dist;
direction = end_sphere_cyl;
}
if (min_squared_dist >= cyl_start_sphere_squared_dist) {
min_squared_dist = cyl_start_sphere_squared_dist;
direction = cyl_start_sphere;
}
if (min_squared_dist >= cyl_end_sphere_squared_dist) {
min_squared_dist = cyl_end_sphere_squared_dist;
direction = cyl_end_sphere;
}
if (min_squared_dist >= cyl_cyl_squared_dist) {
min_squared_dist = cyl_cyl_squared_dist;
direction = cyl_cyl;
}
return sqrt(min_squared_dist) - a_radius - b_radius;
}
Vector4d gc_ppp_pi(Vector3d x1, Vector3d x2, Vector3d x3) {
Vector4d pi;
pi << ( x1 - x3 ).cross( x2 - x3 ), - x3.dot( x1.cross( x2 ) );
return pi;
}
/// TwoPointsCorrelation
unsigned int ICoDF_HTM::HTM::TwoPointsCorrelation(double& radius, double& delta)
{
unsigned int * nbPair = new unsigned int [this->_pointsToCompute.size()];
double infLimit = radius - delta;
if (infLimit < 0) infLimit = 0;
double supLimit = radius + delta;
for (unsigned int i = 0; i < this->_pointsToCompute.size(); ++i)
{
PointInfo_t const * pt = this->_pointsToCompute[i];
nbPair[i] = 0;
if (IsCorrectRA(pt->_ra) && IsCorrectDEC(pt->_dec))
{
Vector3d p = pt->_position;
double pNorm = p.norm();
double pDot = p.dot(Vector3d{1,0,0});
double phi2 = acos(pDot / pNorm);
std::queue<trixel_t*> workingList;
for (unsigned int i = 0; i < 4; ++i)
workingList.push(this->_octahedron->_rootTrixels[i]);
while (workingList.size() > 0)
{
trixel_t* tmp = workingList.front();
workingList.pop();
double dist[3] = {
p.dot(tmp->_vertices[0]),
p.dot(tmp->_vertices[1]),
p.dot(tmp->_vertices[2])
};
unsigned int infInside = (dist[0] > infLimit) + (dist[1] > infLimit) + (dist[2] > infLimit);
unsigned int supInside = (dist[0] > supLimit) + (dist[1] > supLimit) + (dist[2] > supLimit);
if (supInside == 3 && infInside == 0)
nbPair[i] += tmp->_nbChildObject;
else if ((supInside == 3 && infInside > 0)
|| supInside > 0)
{
if (tmp->_children != NULL)
{
for (unsigned int i = 0; i < 4; ++i)
if (tmp->_children[i] != NULL)
workingList.push(tmp->_children[i]);
}
else
nbPair[i] += 1;
}
else
{
if (tmp->_phi == 0)
{
Vector3d tmpVec1 = tmp->_vertices[1] - tmp->_vertices[0];
Vector3d tmpVec2 = tmp->_vertices[2] - tmp->_vertices[1];
Vector3d tmpVec3 = tmpVec1.cross(tmpVec2);
tmp->_trixelBoundary = tmpVec3 / tmpVec3.norm();
tmp->_phi = acos(tmp->_trixelBoundary.dot(Vector3d{1,0,0}) / (tmp->_trixelBoundary.norm()));
tmp->_cross01 = tmp->_vertices[0].cross(tmp->_vertices[1]);
tmp->_cross12 = tmp->_vertices[1].cross(tmp->_vertices[2]);
tmp->_cross20 = tmp->_vertices[2].cross(tmp->_vertices[0]);
}
double theta = tmp->_trixelBoundary.dot(p) / tmp->_trixelBoundary.norm();
if (acos(theta * pNorm) < tmp->_phi + phi2)
{
if (!(tmp->_cross01.dot(p) < 0 &&
tmp->_cross12.dot(p) < 0 &&
tmp->_cross20.dot(p)))
{
nbPair[i] += 1;
}
}
}
}
}
}
delete nbPair;
return std::accumulate(nbPair, nbPair + this->_pointsToCompute.size(), 0);
}
Plane::Plane(const Vector3d& normal, const Vector3d& x)
: m_normal(normal), m_d(normal.dot(x))
{
ensure_invariant();
}
void Homography::
calcFromPlaneParams(const Vector3d& n_c1, const Vector3d& xyz_c1)
{
double d = n_c1.dot(xyz_c1); // normal distance from plane to KF
H_c2_from_c1 = T_c2_from_c1.rotationMatrix() + (T_c2_from_c1.translation()*n_c1.transpose())/d;
}
bool AlignmentAlgorithmPlaneLinear::operator()(AlignmentAlgorithmPlaneLinear::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices)
{
double err=0;
//If my correspondaces indices are less then a minimum threshold, stop it please
if ((int)indices.size()<minimalSetSize()) return false;
//My initial guess for the transformation it's the identity matrix
//maybe in the future i could have a less rough guess
transform = Isometry3d::Identity();
//Unroll the matrix to a vector
Vector12d x=homogeneous2vector(transform.matrix());
Matrix9x1d rx=x.block<9,1>(0,0);
//Initialization of variables, melting fat, i've so much space
Matrix9d H;
H.setZero();
Vector9d b;
b.setZero();
Matrix3x9d A;
//iteration for each correspondace
for (size_t i=0; i<indices.size(); i++)
{
A.setZero();
const Correspondence& c = correspondences[indices[i]];
const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());
//estraggo i vertici dagli edge
const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));
//recupero i dati dei piani
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();
//recupeo le normali, mi servono per condizionare la parte rotazionale
Vector3d ni;
Vector3d nj;
Vector4d coeffs_i=pi.toVector();
Vector4d coeffs_j=pj.toVector();
ni=coeffs_i.head(3);
nj=coeffs_j.head(3);
//inizializzo lo jacobiano, solo la parte rotazionale (per ora)
A.block<1,3>(0,0)=nj.transpose();
A.block<1,3>(1,3)=nj.transpose();
A.block<1,3>(2,6)=nj.transpose();
if(DEBUG) {
cerr << "[DEBUG] PI"<<endl;
cerr << coeffs_i<<endl;
cerr << "[DEBUG] PJ "<<endl;
cerr << coeffs_j<<endl;
cerr << "[ROTATION JACOBIAN][PLANE "<<i<<"]"<<endl;
cerr << A<<endl;
}
//errore
//inizializzo errore
Vector3d ek;
ek.setZero();
ek=A*x.block<9,1>(0,0)-coeffs_i.head(3);
H+=A.transpose()*A;
b+=A.transpose()*ek;
err+=abs(ek.dot(ek));
if(DEBUG)
cerr << "[ROTATIONAL Hessian for plane "<<i<<"]"<<endl<<H<<endl;
if(DEBUG)
cerr << "[ROTATIONAL B for plane "<<i<<"]"<<endl<<b<<endl;
}
LDLT<Matrix9d> ldlt(H);
if (ldlt.isNegative()) return false;
rx=ldlt.solve(-b); // using a LDLT factorizationldlt;
x.block<3,1>(0,0)+=rx.block<3,1>(0,0);
x.block<3,1>(3,0)+=rx.block<3,1>(3,0);
x.block<3,1>(6,0)+=rx.block<3,1>(6,0);
if(DEBUG) {
cerr << "Solving the linear system"<<endl;
cerr << "------------>H"<<endl;
cerr << H<<endl;
cerr << "------------>b"<<endl;
cerr << b<<endl;
cerr << "------------>rx"<<endl;
cerr << rx<<endl;
cerr << "------------>xTEMP"<<endl;
cerr << vector2homogeneous(x)<<endl<<endl;
cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
cerr << "łłłłłłłłłłł Ringraziamo Cthulhu la parte rotazionale è finitałłłłłłłłłłł"<<endl;
cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
}
Matrix4d Xtemp=vector2homogeneous(x);
//now the problem si solved but i could have (and probably i have!)
//a not orthogonal rotation matrix, so i've to recondition it
Matrix3x3d R=Xtemp.block<3,3>(0,0);
// recondition the rotation
JacobiSVD<Matrix3x3d> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV);
if (svd.singularValues()(0)<.5) return false;
R=svd.matrixU()*svd.matrixV().transpose();
Isometry3d X = Isometry3d::Identity();
X.linear()=R;
X.translation()= x.block<3,1>(0,3);
if(DEBUG)
cerr << "X dopo il ricondizionamento appare come:"<<endl;
if(DEBUG)
cerr << X.matrix()<<endl;
Matrix3d H2=Matrix3d::Zero();
Vector3d b2=Vector3d::Zero();
Vector3d A2=Vector3d::Zero();
//at this point rotation is cured, now it's time to work on the translation
for (size_t i=0; i<indices.size(); i++)
{
if(DEBUG)
cerr << "[TRANSLATION JACOBIAN START][PLANE "<<i<<"]"<<endl;
const Correspondence& c = correspondences[indices[i]];
const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());
//estraggo i vertici dagli edge
const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));
//recupero i dati dei piani
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();
//recupeo le normali, mi servono per condizionare la parte rotazionale
Vector3d ni;
Vector3d nj;
Vector4d coeffs_i=pi.toVector();
Vector4d coeffs_j=pj.toVector();
double di;
double dj;
ni=coeffs_i.head(3);
nj=coeffs_j.head(3);
di=coeffs_i(3);
dj=coeffs_j(3);
if(DEBUG)
cerr << "[TRANSLATION JACOBIAN][ JACOBIAN IS: ]"<<endl;
A2=(-nj.transpose()*R.transpose());
if(DEBUG)
cerr << A2<<endl;
double ek;
ek=dj+A2.transpose()*X.translation()-di;
H2+=A2*A2.transpose();
b2+= (A2.transpose()*ek);
err += abs(ek*ek);
if(DEBUG)
cerr << "[TRANSLATIONAL Hessian for plane "<<i<<"]"<<endl<<H2<<endl;
if(DEBUG)
cerr << "[TRANSLATIONAL B for plane "<<i<<"]"<<endl<<b2<<endl;
}
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL Hessian]"<<endl<<H2<<endl;
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL B]"<<endl<<b2<<endl;
//solving the system
Vector3d dt;
LDLT<Matrix3d> ldlt2(H2);
dt=ldlt2.solve(-b2); // using a LDLT factorizationldlt;
if(DEBUG)
cerr << "[FINAL][TRANSLATIONAL DT]"<<endl<<dt<<endl;
X.translation()+=dt;
transform = X;
if(DEBUG)
{
cerr << "TRANSFORM found: " << endl<< endl<< endl;
cerr << g2o::internal::toVectorMQT(X) << endl;;
cerr << transform.matrix()<< endl;;
}
return true;
}
//-------------------------------------------------------------------------------------------------------
bool Exporter::prepareVertexStreams()
{
auto& textureCoordinates = meshData_->vertices[Mesh::VERTEX_STREAM_TEXTURE_COORDINATES];
auto& positions = meshData_->vertices[Mesh::VERTEX_STREAM_POSITIONS];
auto& tbnBases = meshData_->vertices[Mesh::VERTEX_STREAM_TBN_BASES];
auto& boneIndicesAndWeights = meshData_->vertices[Mesh::VERTEX_STREAM_BONE_INDICES_AND_WEIGHTS];
if(!textureCoordinates.create(numVertices_, sizeof(Vector2d)) ||
!positions.create(numVertices_, sizeof(Vector3d)) ||
!tbnBases.create(numVertices_, sizeof(Vector3d) + sizeof(Vector4d)))
return false;
if(!rawMesh_->bones_.isEmpty())
{
if(!boneIndicesAndWeights.create(numVertices_, 2 * sizeof(Vector4d)))
return false;
}
for(uint32_t i = 0; i < faces_.getSize(); ++i)
{
for(uint8_t j = 0; j < 3; ++j)
{
uint32_t vertexIndex = faces_[i].indices[j];
const auto& vertex = vertices_[vertexIndex];
uint8_t* stream = &positions[vertexIndex * positions.getStride()];
*reinterpret_cast<Vector3d*>(stream) = vertex.pos;
stream = &textureCoordinates[vertexIndex * textureCoordinates.getStride()];
*reinterpret_cast<Vector2d*>(stream) = Vector2d(vertex.s, vertex.t);
stream = &tbnBases[vertexIndex * tbnBases.getStride()];
Vector4d& tangent = *(reinterpret_cast<Vector4d*>(stream + sizeof(Vector3d)));
Vector3d& normal = *(reinterpret_cast<Vector3d*>(stream));
normal = vertex.normal;
normal.normalize();
// tangent and binormal
Vector3d t = vertex.tangent;
Vector3d b = vertex.binormal;
t = t - normal.dot(t) * normal;
t.normalize();
b = b - normal.dot(b) * normal - t.dot(b) * t / t.dot(t);
b.normalize();
Vector3d crossProduct = normal.cross(t);
if(crossProduct == b)
tangent.define(t, 1.0f);
else if(crossProduct == -b)
tangent.define(t, -1.0f);
else
{
std::cout << "warning: vertex " << vertexIndex <<
" has bad TBN basis" << std::endl;
tangent = Vector4d(t, -1.0f);
}
if(!rawMesh_->bones_.isEmpty())
{
stream = &boneIndicesAndWeights[vertexIndex *
boneIndicesAndWeights.getStride()];
Vector4d& boneIndices = *(reinterpret_cast<Vector4d*>(stream));
Vector4d& boneWeights = *(reinterpret_cast<Vector4d*>(stream +
sizeof(Vector4d)));
uint32_t oldVertexIndex = newToOldVertexMapping_[vertexIndex];
boneIndices = boneIndices_[oldVertexIndex];
boneWeights = boneWeights_[oldVertexIndex];
if(std::fabs(boneWeights.x + boneWeights.y +
boneWeights.z + boneWeights.w - 1.0f) > SELENE_EPSILON)
std::cout << "warning: vertex " << vertexIndex <<
" has bad bone weights" << std::endl;
}
}
}
return true;
}
void
NumericalSolver::calcAccel( Vector3d *inPositions, Vector3d *inVelocities,
double *inInvMasses, Vector3d *outAccel )
{
Vector3d gravity(0,0,0);
idx_t numParticles = mParticleSystem->getNumParticles();
gravity.pY = -(mParticleSystem->getGravity());
//---------- Initialize Particle Forces to wind force ----------
Vector3d Wind = mParticleSystem->getNewWind();
Vector3d NormalizedWind = Wind.normalized();
double dotp;
for( int i = 0; i < numParticles; i++ )
{
Vector3d &normal = mParticleSystem->getParticleNormal(i);
dotp = NormalizedWind.dot(normal);
if (dotp < 0)
dotp *= -1;
outAccel[i] = Wind*dotp;
}
//---------- Sum Forces ----------
for( SpringListIt it = mParticleSystem->getSprings().begin();
it != mParticleSystem->getSprings().end(); it++ )
{
Spring &theSpring = *it;
idx_t aIdx = theSpring.getParticleA();
idx_t bIdx = theSpring.getParticleB();
Vector3d dx = (inPositions[aIdx]-inPositions[bIdx]);
Vector3d dv = (inVelocities[aIdx]-inVelocities[bIdx]);
double xLen = dx.length();
double len = xLen - theSpring.getRestLength();
//avoid divide by zero
if( ABS(xLen) < NEAR_ZERO || ABS(len) < NEAR_ZERO )
continue;
//don't allow springs to stretch more than twice their original length
// if( len > (3 * theSpring.getRestLength()) )
// continue;
Vector3d F = (dx/xLen)*(len*theSpring.getK());
Vector3d FB = (dx/xLen)*(dv.dot(dx)/xLen)*theSpring.getB();
if ((F.normalized()+FB.normalized()).length() < 0.1)
{
if (FB.length() > F.length())
continue;
else
F += FB;
}
/* //used for debugging
if (theSpring.mType == 1)
{
continue;
}
if (theSpring.mType == 2)
{
continue;
}
*/
//force affects both springs in opposite directions
outAccel[aIdx] -= F;
outAccel[bIdx] += F;
}
//----------------- calculate Acceleration -------------------
for( int i = 0; i < numParticles; i++ )
{
outAccel[i] = gravity + (outAccel[i] * inInvMasses[i]);
}
}
Vector2d
FittingCylinder::inverseMapping (const ON_NurbsSurface &nurbs, const Vector3d &pt, const Vector2d &hint, double &error,
Vector3d &p, Vector3d &tu, Vector3d &tv, int maxSteps, double accuracy, bool quiet)
{
double pointAndTangents[9];
Vector2d current, delta;
Matrix2d A;
Vector2d b;
Vector3d r;
std::vector<double> elementsU = getElementVector (nurbs, 0);
std::vector<double> elementsV = getElementVector (nurbs, 1);
double minU = elementsU[0];
double minV = elementsV[0];
double maxU = elementsU[elementsU.size () - 1];
double maxV = elementsV[elementsV.size () - 1];
current = hint;
for (int k = 0; k < maxSteps; k++)
{
nurbs.Evaluate (current (0), current (1), 1, 3, pointAndTangents);
p (0) = pointAndTangents[0];
p (1) = pointAndTangents[1];
p (2) = pointAndTangents[2];
tu (0) = pointAndTangents[3];
tu (1) = pointAndTangents[4];
tu (2) = pointAndTangents[5];
tv (0) = pointAndTangents[6];
tv (1) = pointAndTangents[7];
tv (2) = pointAndTangents[8];
r = p - pt;
b (0) = -r.dot (tu);
b (1) = -r.dot (tv);
A (0, 0) = tu.dot (tu);
A (0, 1) = tu.dot (tv);
A (1, 0) = A (0, 1);
A (1, 1) = tv.dot (tv);
delta = A.ldlt ().solve (b);
if (delta.norm () < accuracy)
{
error = r.norm ();
return current;
}
else
{
current = current + delta;
if (current (0) < minU)
current (0) = minU;
else if (current (0) > maxU)
current (0) = maxU;
if (current (1) < minV)
current (1) = maxV - (minV - current (1));
else if (current (1) > maxV)
current (1) = minV + (current (1) - maxV);
}
}
error = r.norm ();
if (!quiet)
{
printf ("[FittingCylinder::inverseMapping] Warning: Method did not converge (%e %d)\n", accuracy, maxSteps);
printf (" %f %f ... %f %f\n", hint (0), hint (1), current (0), current (1));
}
return current;
}
int main(int argc, const char* argv[])
{
if (argc<2)
{
cout << endl;
cout << "Please type: " << endl;
cout << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] [STRUCTURE_ONLY] [DENSE]" << endl;
cout << endl;
cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
cout << "OUTLIER_RATIO: probability of spuroius observation (default: 0.0)" << endl;
cout << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)" << endl;
cout << "STRUCTURE_ONLY: performe structure-only BA to get better point initializations (0 or 1; default: 0==false)" << endl;
cout << "DENSE: Use dense solver (0 or 1; default: 0==false)" << endl;
cout << endl;
cout << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to 1==true." << endl;
cout << endl;
exit(0);
}
double PIXEL_NOISE = atof(argv[1]);
double OUTLIER_RATIO = 0.0;
if (argc>2)
{
OUTLIER_RATIO = atof(argv[2]);
}
bool ROBUST_KERNEL = false;
if (argc>3)
{
ROBUST_KERNEL = atoi(argv[3]) != 0;
}
bool STRUCTURE_ONLY = false;
if (argc>4)
{
STRUCTURE_ONLY = atoi(argv[4]) != 0;
}
bool DENSE = false;
if (argc>5)
{
DENSE = atoi(argv[5]) != 0;
}
cout << "PIXEL_NOISE: " << PIXEL_NOISE << endl;
cout << "OUTLIER_RATIO: " << OUTLIER_RATIO<< endl;
cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl;
cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl;
cout << "DENSE: "<< DENSE << endl;
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
if (DENSE)
{
linearSolver= new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
cerr << "Using DENSE" << endl;
}
else
{
#ifdef G2O_HAVE_CHOLMOD
cerr << "Using CHOLMOD" << endl;
linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>();
#elif defined G2O_HAVE_CSPARSE
linearSolver = new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
cerr << "Using CSPARSE" << endl;
#else
#error neither CSparse nor Cholmod are available
#endif
}
g2o::BlockSolver_6_3 * solver_ptr
= new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// set up 500 points
vector<Vector3d> true_points;
for (size_t i=0;i<500; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
Vector2d focal_length(500,500); // pixels
Vector2d principal_point(320,240); // 640x480 image
double baseline = 0.075; // 7.5 cm baseline
vector<g2o::SE3Quat,
aligned_allocator<g2o::SE3Quat> > true_poses;
// set up camera params
g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
principal_point[0],principal_point[1],
baseline);
// set up 5 vertices, first 2 fixed
int vertex_id = 0;
for (size_t i=0; i<5; ++i)
{
Vector3d trans(i*0.04-1.,0,0);
Eigen:: Quaterniond q;
q.setIdentity();
g2o::SE3Quat pose(q,trans);
g2o::VertexSCam * v_se3
= new g2o::VertexSCam();
v_se3->setId(vertex_id);
v_se3->setEstimate(pose);
v_se3->setAll(); // set aux transforms
if (i<2)
v_se3->setFixed(true);
optimizer.addVertex(v_se3);
true_poses.push_back(pose);
vertex_id++;
}
int point_id=vertex_id;
int point_num = 0;
double sum_diff2 = 0;
cout << endl;
tr1::unordered_map<int,int> pointid_2_trueid;
tr1::unordered_set<int> inliers;
// add point projections to this vertex
for (size_t i=0; i<true_points.size(); ++i)
{
g2o::VertexSBAPointXYZ * v_p
= new g2o::VertexSBAPointXYZ();
v_p->setId(point_id);
v_p->setMarginalized(true);
v_p->setEstimate(true_points.at(i)
+ Vector3d(Sample::gaussian(1),
Sample::gaussian(1),
Sample::gaussian(1)));
int num_obs = 0;
for (size_t j=0; j<true_poses.size(); ++j)
{
Vector3d z;
dynamic_cast<g2o::VertexSCam*>
(optimizer.vertices().find(j)->second)
->mapPoint(z,true_points.at(i));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
{
++num_obs;
}
}
if (num_obs>=2)
{
optimizer.addVertex(v_p);
bool inlier = true;
for (size_t j=0; j<true_poses.size(); ++j)
{
Vector3d z;
dynamic_cast<g2o::VertexSCam*>
(optimizer.vertices().find(j)->second)
->mapPoint(z,true_points.at(i));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
{
double sam = Sample::uniform();
if (sam<OUTLIER_RATIO)
{
z = Vector3d(Sample::uniform(64,640),
Sample::uniform(0,480),
Sample::uniform(0,64)); // disparity
z(2) = z(0) - z(2); // px' now
inlier= false;
}
z += Vector3d(Sample::gaussian(PIXEL_NOISE),
Sample::gaussian(PIXEL_NOISE),
Sample::gaussian(PIXEL_NOISE/16.0));
g2o::Edge_XYZ_VSC * e
= new g2o::Edge_XYZ_VSC();
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertices().find(j)->second);
e->setMeasurement(z);
//e->inverseMeasurement() = -z;
e->information() = Matrix3d::Identity();
e->setRobustKernel(ROBUST_KERNEL);
e->setHuberWidth(1);
optimizer.addEdge(e);
}
}
if (inlier)
{
inliers.insert(point_id);
Vector3d diff = v_p->estimate() - true_points[i];
sum_diff2 += diff.dot(diff);
}
// else
// cout << "Point: " << point_id << "has at least one spurious observation" <<endl;
pointid_2_trueid.insert(make_pair(point_id,i));
++point_id;
++point_num;
}
}
cout << endl;
optimizer.initializeOptimization();
optimizer.setVerbose(true);
if (STRUCTURE_ONLY)
{
cout << "Performing structure-only BA:" << endl;
g2o::StructureOnlySolver<3> structure_only_ba;
g2o::OptimizableGraph::VertexContainer points;
for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
if (v->dimension() == 3)
points.push_back(v);
}
structure_only_ba.calc(points, 10);
}
cout << endl;
cout << "Performing full BA:" << endl;
optimizer.optimize(10);
cout << endl;
cout << "Point error before optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
point_num = 0;
sum_diff2 = 0;
for (tr1::unordered_map<int,int>::iterator it=pointid_2_trueid.begin();
it!=pointid_2_trueid.end(); ++it)
{
g2o::HyperGraph::VertexIDMap::iterator v_it
= optimizer.vertices().find(it->first);
if (v_it==optimizer.vertices().end())
{
cerr << "Vertex " << it->first << " not in graph!" << endl;
exit(-1);
}
g2o::VertexSBAPointXYZ * v_p
= dynamic_cast< g2o::VertexSBAPointXYZ * > (v_it->second);
if (v_p==0)
{
cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
exit(-1);
}
Vector3d diff = v_p->estimate()-true_points[it->second];
if (inliers.find(it->first)==inliers.end())
continue;
sum_diff2 += diff.dot(diff);
++point_num;
}
cout << "Point error after optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
cout << endl;
}
/// Determines the distance between a line and a line segment
double Triangle::calc_sq_dist(const Point3d& origin, const Vector3d& dir, const LineSeg3& seg, Point3d& cp_line, Point3d& cp_seg, double& t_line, double& t_seg)
{
// determine the origins of the segment
Point3d seg_origin = (seg.first + seg.second)*0.5;
// determine the directions of the segment
Point3d seg_dir = seg.second - seg.first;
double seg_dir_len = seg_dir.norm();
seg_dir /= seg_dir_len;
// determine the extents of the segment
double seg_extent = seg_dir_len * 0.5;
Vector3d kDiff = origin - seg_origin;
double fA01 = -dir.dot(seg_dir);
double fB0 = kDiff.dot(dir);
double fC = kDiff.norm_sq();
double fDet = std::fabs(1.0 - fA01*fA01);
double fB1, fS0, fS1, fSqrDist, fExtDet;
if (fDet >= NEAR_ZERO)
{
// The line and segment are not parallel.
fB1 = -kDiff.dot(seg_dir);
fS1 = fA01*fB0-fB1;
fExtDet = seg_extent*fDet;
if (fS1 >= -fExtDet)
{
if (fS1 <= fExtDet)
{
// Two interior points are closest, one on the line and one
// on the segment.
double fInvDet = (1.0)/fDet;
fS0 = (fA01*fB1-fB0)*fInvDet;
fS1 *= fInvDet;
fSqrDist = fS0*(fS0+fA01*fS1+(2.0)*fB0)+fS1*(fA01*fS0+fS1+(2.0)*fB1)+fC;
}
else
{
// The end point e1 of the segment and an interior point of
// the line are closest.
fS1 = seg_extent;
fS0 = -(fA01*fS1+fB0);
fSqrDist = -fS0*fS0+fS1*(fS1+(2.0)*fB1)+fC;
}
}
else
{
// The end point e0 of the segment and an interior point of the
// line are closest.
fS1 = -seg_extent;
fS0 = -(fA01*fS1+fB0);
fSqrDist = -fS0*fS0+fS1*(fS1+(2.0)*fB1)+fC;
}
}
else
{
// The line and segment are parallel. Choose the closest pair so that
// one point is at segment origin.
fS1 = 0.0;
fS0 = -fB0;
fSqrDist = fB0*fS0+fC;
}
cp_line = origin + fS0*dir;
cp_seg = seg_origin + fS1*seg_dir;
t_line = fS0;
t_seg = fS1;
return std::fabs(fSqrDist);
}
/** Determine the closest object intersected by a ray given by the pick origin and direction.
* This method returns true if any object was intersected. If the value of result is not
* null, it will be updated with information about which object was hit by the pick ray.
*
* @param t The time given as the number of seconds since 1 Jan 2000 12:00:00 UTC.
* @param pickOrigin origin of the pick ray
* @param pickDirection direction of the pick ray (does not need to be normalized)
* @param pixelAngle angle in radians subtended by a pixel
* @param result pointer to a PickResult object that will be filled in if the pick ray
* hits something.
*/
bool
Universe::pickObject(double t,
const Vector3d& pickOrigin,
const Vector3d& pickDirection,
double pixelAngle,
PickResult* result) const
{
double closest = numeric_limits<double>::infinity();
PickResult closestResult;
for (EntityTable::const_iterator iter = m_entities.begin(); iter != m_entities.end(); ++iter)
{
Entity* entity = iter->ptr();
if (entity->geometry() || entity->hasVisualizers())
{
if (entity->isVisible() && entity->chronology()->includesTime(t))
{
Vector3d position = entity->position(t);
if (entity->geometry())
{
Geometry* geometry = entity->geometry();
double intersectionDistance;
if (TestRaySphereIntersection(pickOrigin,
pickDirection,
position,
geometry->boundingSphereRadius(),
&intersectionDistance))
{
if (intersectionDistance < closest)
{
// Transform the pick ray into the local coordinate system of body
Matrix3d invRotation = entity->orientation(t).conjugate().toRotationMatrix();
Vector3d relativePickOrigin = invRotation * (pickOrigin - position);
Vector3d relativePickDirection = invRotation * pickDirection;
double distance = intersectionDistance;
if (geometry->rayPick(relativePickOrigin, relativePickDirection, t, &distance))
{
if (distance < closest)
{
closest = distance;
closestResult.setHit(entity, distance, pickOrigin + pickDirection * distance);
}
}
}
}
}
// Visualizers may act as 'pick proxies'
if (entity->hasVisualizers())
{
Vector3d relativePickOrigin = pickOrigin - position;
// Calculate the distance to the plane containing the center of the visualizer
// and perpendicular to the pick direction.
double distanceToPlane = -pickDirection.dot(relativePickOrigin);
if (distanceToPlane > 0.0 && distanceToPlane < closest)
{
for (Entity::VisualizerTable::const_iterator iter = entity->visualizers()->begin();
iter != entity->visualizers()->end(); ++iter)
{
const Visualizer* visualizer = iter->second.ptr();
if (visualizer->isVisible() &&
visualizer->rayPick(relativePickOrigin, pickDirection, pixelAngle))
{
closest = distanceToPlane;
closestResult.setHit(entity, distanceToPlane, pickOrigin + pickDirection * distanceToPlane);
break;
}
}
}
}
}
}
}
if (closest < numeric_limits<double>::infinity())
{
if (result)
{
*result = closestResult;
}
return true;
}
else
{
return false;
}
}