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; } }