Point2 Exact_adaptive_kernel::circumcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3) { Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y()); Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y()); Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y()); double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y(); double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y(); double denominator = 0.5/(2.0*signed_area(p1, p2, p3)); BOOST_ASSERT(denominator > 0.0); double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator; double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator; return Point2(p1.x()+dx, p1.y()+dy); }
bool IsInsideTriangle(const ISectTriangle* triangle, MPMesh* pMesh, MPMesh::FaceHandle coTri, const Point2 &bc, Relation &rel) { Vec3d *v0, *v1, *v2; GetCorners(pMesh, coTri, v0, v1, v2); OpenMesh::Vec2d v[3]; v[0][0] = (*v0)[triangle->xi]; v[0][1] = (*v0)[triangle->yi]; v[1][0] = (*v1)[triangle->xi]; v[1][1] = (*v1)[triangle->yi]; v[2][0] = (*v2)[triangle->xi]; v[2][1] = (*v2)[triangle->yi]; if (IsPointInTriangle(OpenMesh::Vec2d(bc.x(), bc.y()), v, rel)) { double d = OpenMesh::dot(triangle->pMesh->normal(triangle->face), pMesh->normal(coTri)); if (pMesh->bInverse ^ triangle->pMesh->bInverse) d = -d; if (d > 0.0) rel = REL_SAME; else rel = REL_OPPOSITE; return true; } else return false; }
void Object::getChildrenDimensions(Point2& r_pos, Point2& r_size) { Point2 min(0, 0); Point2 max = size; if(hasChildren()) { BOOST_FOREACH(const PTR(Object)& a, children) { Point2 p, s; a->getChildrenDimensions(p, s); float x1 = p.x(); float x2 = p.x() + s.x(); float y1 = p.y(); float y2 = p.y() + s.y(); Danvilifmin(min.x(), x1); Danvilifmax(max.x(), x2); Danvilifmin(min.y(), y1); Danvilifmax(max.y(), y2); } } r_pos = min + pos; r_size = max - min; }
/* ************************************************************************* */ Point2 CalibratedCamera::project(const Point3& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { #ifdef CALIBRATEDCAMERA_CHAIN_RULE Point3 q = pose_.transform_to(point, H1, H2); #else Point3 q = pose_.transform_to(point); #endif Point2 intrinsic = project_to_camera(q); // Check if point is in front of camera if(q.z() <= 0) throw CheiralityException(); if (H1 || H2) { #ifdef CALIBRATEDCAMERA_CHAIN_RULE // just implement chain rule Matrix H; project_to_camera(q,H); if (H1) *H1 = H * (*H1); if (H2) *H2 = H * (*H2); #else // optimized version, see CalibratedCamera.nb const double z = q.z(), d = 1.0/z; const double u = intrinsic.x(), v = intrinsic.y(), uv = u*v; if (H1) *H1 = Matrix_(2,6, uv,-(1.+u*u), v, -d , 0., d*u, (1.+v*v), -uv,-u, 0.,-d , d*v ); if (H2) { const Matrix R(pose_.rotation().matrix()); *H2 = d * Matrix_(2,3, R(0,0) - u*R(0,2), R(1,0) - u*R(1,2), R(2,0) - u*R(2,2), R(0,1) - v*R(0,2), R(1,1) - v*R(1,2), R(2,1) - v*R(2,2) ); } #endif } return intrinsic; }
/* ************************************************************************* */ Point2 Cal3Bundler::uncalibrate(const Point2& p, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { // r = x^2 + y^2 ; // g = (1 + k(1)*r + k(2)*r^2) ; // pi(:,i) = g * pn(:,i) const double x = p.x(), y = p.y() ; const double r = x*x + y*y ; const double r2 = r*r ; const double g = 1 + k1_ * r + k2_*r2 ; // save one multiply const double fg = f_*g ; // semantic meaningful version //if (H1) *H1 = D2d_calibration(p) ; //if (H2) *H2 = D2d_intrinsic(p) ; // unrolled version, much faster if ( H1 || H2 ) { const double fx = f_*x, fy = f_*y ; if ( H1 ) { *H1 = Matrix_(2, 3, g*x, fx*r , fx*r2, g*y, fy*r , fy*r2) ; } if ( H2 ) { const double dr_dx = 2*x ; const double dr_dy = 2*y ; const double dg_dx = k1_*dr_dx + k2_*2*r*dr_dx ; const double dg_dy = k1_*dr_dy + k2_*2*r*dr_dy ; *H2 = Matrix_(2, 2, fg + fx*dg_dx, fx*dg_dy, fy*dg_dx , fg + fy*dg_dy) ; } } return Point2(fg * x, fg * y) ; }
/* ************************************************************************* */ bool SimPolygon2D::contains(const Point2& c) const { vector<SimWall2D> edges = walls(); bool initialized = false; bool lastSide = false; BOOST_FOREACH(const SimWall2D& ab, edges) { // compute cross product of ab and ac Point2 dab = ab.b() - ab.a(); Point2 dac = c - ab.a(); double cross = dab.x() * dac.y() - dab.y() * dac.x(); if (fabs(cross) < 1e-6) // check for on one of the edges return true; bool side = cross > 0; // save the first side found if (!initialized) { lastSide = side; initialized = true; continue; } // to be inside the polygon, point must be on the same side of all lines if (lastSide != side) return false; }
Point2 Exact_adaptive_kernel::offcenter(Point2 const& p1, Point2 const& p2, Point2 const& p3, double offconstant) { Point2 p2p1(p2.x()-p1.x(), p2.y()-p1.y()); Point2 p3p1(p3.x()-p1.x(), p3.y()-p1.y()); Point2 p2p3(p2.x()-p3.x(), p2.y()-p3.y()); double p2p1dist = p2p1.x()*p2p1.x() + p2p1.y()*p2p1.y(); double p3p1dist = p3p1.x()*p3p1.x() + p3p1.y()*p3p1.y(); double p2p3dist = p2p3.x()*p2p3.x() + p2p3.y()*p2p3.y(); double denominator = 0.5/(2.0*Exact_adaptive_kernel::signed_area(p1, p2, p3)); BOOST_ASSERT(denominator > 0.0); double dx = (p3p1.y() * p2p1dist - p2p1.y() * p3p1dist) * denominator; double dy = (p2p1.x() * p3p1dist - p3p1.x() * p2p1dist) * denominator; double dxoff, dyoff; if ((p2p1dist < p3p1dist) && (p2p1dist < p2p3dist)) { dxoff = 0.5 * p2p1.x() - offconstant * p2p1.y(); dyoff = 0.5 * p2p1.y() + offconstant * p2p1.x(); if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) { dx = dxoff; dy = dyoff; } } else if (p3p1dist < p2p3dist) { dxoff = 0.5 * p3p1.x() + offconstant * p3p1.y(); dyoff = 0.5 * p3p1.y() - offconstant * p3p1.x(); if (dxoff * dxoff + dyoff * dyoff < dx * dx + dy * dy) { dx = dxoff; dy = dyoff; } } else { dxoff = 0.5 * p2p3.x() - offconstant * p2p3.y(); dyoff = 0.5 * p2p3.y() + offconstant * p2p3.x(); if (dxoff * dxoff + dyoff * dyoff < (dx - p2p1.x()) * (dx - p2p1.x()) + (dy - p2p1.y()) * (dy - p2p1.y())) { dx = p2p1.x() + dxoff; dy = p2p1.y() + dyoff; } } return Point2(p1.x()+dx, p1.y()+dy); }
// main int main(int argc, char** argv) { // load Plaza1 data list<TimedOdometry> odometry = readOdometry(); // size_t M = odometry.size(); vector<RangeTriple> triples = readTriples(); size_t K = triples.size(); // parameters size_t start = 220, end=3000; size_t minK = 100; // first batch of smart factors size_t incK = 50; // minimum number of range measurements to process after bool robust = true; bool smart = true; // Set Noise parameters Vector priorSigmas = Vector3(1, 1, M_PI); Vector odoSigmas = Vector3(0.05, 0.01, 0.2); double sigmaR = 100; // range standard deviation const NM::Base::shared_ptr // all same type priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust rangeNoise = robust ? tukey : gaussian; // Initialize iSAM ISAM2 isam; // Add prior on first pose Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, M_PI - 2.02108900000000); NonlinearFactorGraph newFactors; newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise)); ofstream os2( "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt"); ofstream os3( "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt"); // initialize points (Gaussian) Values initial; if (!smart) { initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778)); initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278)); initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678)); initial.insert(symbol('L', 5), Point2(1.7095, -5.8122)); } Values landmarkEstimates = initial; // copy landmarks initial.insert(0, pose0); // initialize smart range factors size_t ids[] = { 1, 6, 0, 5 }; typedef boost::shared_ptr<SmartRangeFactor> SmartPtr; map<size_t, SmartPtr> smartFactors; if (smart) { for(size_t jj: ids) { smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR)); newFactors.push_back(smartFactors[jj]); } } // set some loop variables size_t i = 1; // step counter size_t k = 0; // range measurement counter Pose2 lastPose = pose0; size_t countK = 0, totalCount=0; // Loop over odometry gttic_(iSAM); for(const TimedOdometry& timedOdometry: odometry) { //--------------------------------- odometry loop ----------------------------------------- double t; Pose2 odometry; boost::tie(t, odometry) = timedOdometry; // add odometry factor newFactors.push_back( BetweenFactor<Pose2>(i - 1, i, odometry, NM::Diagonal::Sigmas(odoSigmas))); // predict pose and add as initial estimate Pose2 predictedPose = lastPose.compose(odometry); lastPose = predictedPose; initial.insert(i, predictedPose); landmarkEstimates.insert(i, predictedPose); // Check if there are range factors to be added while (k < K && t >= boost::get<0>(triples[k])) { size_t j = boost::get<1>(triples[k]); double range = boost::get<2>(triples[k]); if (i > start) { if (smart && totalCount < minK) { smartFactors[j]->addRange(i, range); printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl; } else { RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range, rangeNoise); // Throw out obvious outliers based on current landmark estimates Vector error = factor.unwhitenedError(landmarkEstimates); if (k <= 200 || fabs(error[0]) < 5) newFactors.push_back(factor); } totalCount += 1; } k = k + 1; countK = countK + 1; } // Check whether to update iSAM 2 if (k >= minK && countK >= incK) { gttic_(update); isam.update(newFactors, initial); gttoc_(update); gttic_(calculateEstimate); Values result = isam.calculateEstimate(); gttoc_(calculateEstimate); lastPose = result.at<Pose2>(i); bool hasLandmarks = result.exists(symbol('L', ids[0])); if (hasLandmarks) { // update landmark estimates landmarkEstimates = Values(); for(size_t jj: ids) landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj))); } newFactors = NonlinearFactorGraph(); initial = Values(); if (smart && !hasLandmarks) { cout << "initialize from smart landmarks" << endl; for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); initial.insert(symbol('L', jj), landmark); landmarkEstimates.insert(symbol('L', jj), landmark); } } countK = 0; for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>()) os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1" << endl; if (smart) { for(size_t jj: ids) { Point2 landmark = smartFactors[jj]->triangulate(result); os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1" << endl; } } } i += 1; if (i>end) break; //--------------------------------- odometry loop ----------------------------------------- } // end for gttoc_(iSAM); // Print timings tictoc_print_(); // Write result to file Values result = isam.calculateEstimate(); ofstream os( "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt"); for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>()) os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t" << it.value.theta() << endl; exit(0); }
/* ************************************************************************* */ Point3 CalibratedCamera::backproject_from_camera(const Point2& p, const double scale) { return Point3(p.x() * scale, p.y() * scale, scale); }
/****************************************************************************** * Renders the text string at the given location given in normalized * viewport coordinates ([-1,+1] range). ******************************************************************************/ void DefaultTextPrimitive::renderViewport(SceneRenderer* renderer, const Point2& pos, int alignment) { QSize imageSize = renderer->outputSize(); Point2 windowPos((pos.x() + 1.0f) * imageSize.width() / 2, (-pos.y() + 1.0f) * imageSize.height() / 2); renderWindow(renderer, windowPos, alignment); }
/// \brief Check a point is outside this clip. /// /// @param p point to be checked. /// @return true if p is outside the clip. bool inside(const Point2& p) const { return p.x() < rightX; }
/// \brief Check a point is outside this clip. /// /// @param p point to be checked. /// @return true if p is outside the clip. bool inside(const Point2& p) const { return p.x() >= leftX; }
inline cv::Point2f tocv (Point2 &p) { return cv::Point2f (p.x(), p.y()); }
BOOST_FOREACH(const Point2Pair& pair, pairs) { Point2 dq = pair.first - cp; Point2 dp = pair.second - cq; c += dp.x() * dq.x() + dp.y() * dq.y(); s += dp.y() * dq.x() - dp.x() * dq.y(); // this works but is negative from formula above !! :-( }