void Transform::SetTransform(const Pose2& rPose1, const Pose2& rPose2) { if (rPose1 == rPose2) { m_Rotation.SetToIdentity(); m_InverseRotation.SetToIdentity(); m_Transform = Pose2(); return; } // heading transformation m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading()); m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading()); // position transformation Pose2 newPosition; if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0) { newPosition = rPose2 - m_Rotation * rPose1; } else { newPosition = rPose2; } m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading()); }
/* ************************************************************************* */ Vector Pose2BetweenFactorEvaluateErrorDefault(const Pose2& measured, const Pose2& p1, const Pose2& p2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) { Pose2 hx = p1.between(p2, H1, H2); // h(x) // manifold equivalent of h(x)-z -> log(z,h(x)) return measured.localCoordinates(hx); }
/* ************************************************************************* */ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) { // get cosines and sines from rotation matrices const Rot2& R1 = r1.r(), R2 = r2.r(); double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s(); // Assert that R1 and R2 are normalized assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5); // Calculate delta rotation = between(R1,R2) double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2; Rot2 R(Rot2::atan2(s,c)); // normalizes // Calculate delta translation = unrotate(R1, dt); Point2 dt = r2.t() - r1.t(); double x = dt.x(), y = dt.y(); Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y); // FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; *H1 = Matrix3(); (*H1) << -c, -s, dt1, s, -c, dt2, 0.0, 0.0,-1.0; } if (H2) *H2 = Matrix3::Identity(); return Pose2(R,t); }
/* ************************************************************************* */ int main() { int n = 50000000; cout << "NOTE: Times are reported for " << n << " calls" << endl; // create a random pose: double x = 4.0, y = 2.0, r = 0.3; Vector v = (Vector(3) << x, y, r).finished(); Pose2 X = Pose2(3,2,0.4), X2 = X.retract(v), X3(5,6,0.3); TEST(Expmap, Pose2::Expmap(v)); TEST(Retract, X.retract(v)); TEST(Logmap, Pose2::Logmap(X2)); TEST(localCoordinates, X.localCoordinates(X2)); Matrix H1, H2; Matrix3 H1f, H2f; TEST(Pose2BetweenFactorEvaluateErrorDefault, Pose2BetweenFactorEvaluateErrorDefault(X3, X, X2, H1, H2)); TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetween, Pose2BetweenFactorEvaluateErrorOptimizedBetween(X3, X, X2, H1, H2)); TEST(Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed, Pose2BetweenFactorEvaluateErrorOptimizedBetweenFixed(X3, X, X2, H1f, H2f)); // Print timings tictoc_print_(); return 0; }
/* ************************************************************************* */ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) { double st = sin(pose2.theta()), ct = cos(pose2.theta()); Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); Rot3 wRc(x, y, z); Point3 t(pose2.x(), pose2.y(), height); Pose3 pose3(wRc, t); return CalibratedCamera(pose3); }
/* ************************************************************************* */ Vector Pose2::localCoordinates(const Pose2& p2) const { #ifdef SLOW_BUT_CORRECT_EXPMAP return Logmap(between(p2)); #else Pose2 r = between(p2); return (Vector(3) << r.x(), r.y(), r.theta()); #endif }
/* ************************************************************************* */ Rot2 Pose2::bearing(const Pose2& point, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { Rot2 result = bearing(point.t(), H1, H2); if (H2) { Matrix H2_ = *H2 * point.r().matrix(); *H2 = zeros(1, 3); insertSub(*H2, H2_, 0, 0); } return result; }
// Using the NoiseModelFactor1 base class there are two functions that must be overridden. // The first is the 'evaluateError' function. This function implements the desired measurement // function, returning a vector of errors when evaluated at the provided variable value. It // must also calculate the Jacobians for this measurement function, if requested. Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const { // The measurement function for a GPS-like measurement is simple: // error_x = pose.x - measurement.x // error_y = pose.y - measurement.y // Consequently, the Jacobians are: // [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0] // [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0] if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished(); return (Vector(2) << q.x() - mx_, q.y() - my_).finished(); }
/* ************************************************************************* */ Vector Pose2::Logmap(const Pose2& p) { const Rot2& R = p.r(); const Point2& t = p.t(); double w = R.theta(); if (std::abs(w) < 1e-10) return (Vector(3) << t.x(), t.y(), w); else { double c_1 = R.c()-1.0, s = R.s(); double det = c_1*c_1 + s*s; Point2 p = R_PI_2 * (R.unrotate(t) - t); Point2 v = (w/det) * p; return (Vector(3) << v.x(), v.y(), w); } }
// see doc/math.lyx, SE(2) section Pose2 Pose2::compose(const Pose2& p2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { // TODO: inline and reuse? if(H1) *H1 = p2.inverse().AdjointMap(); if(H2) *H2 = I3; return (*this)*p2; }
/* ************************************************************************* */ double Pose2::range(const Pose2& pose2, boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { Point2 d = pose2.t() - t_; if (!H1 && !H2) return d.norm(); Matrix H; double r = d.norm(H); if (H1) *H1 = H * (Matrix(2, 3) << -r_.c(), r_.s(), 0.0, -r_.s(), -r_.c(), 0.0); if (H2) *H2 = H * (Matrix(2, 3) << pose2.r_.c(), -pose2.r_.s(), 0.0, pose2.r_.s(), pose2.r_.c(), 0.0); return r; }
// 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); }
// main int main (int argc, char** argv) { // load Plaza2 data list<TimedOdometry> odometry = readOdometry(); // size_t M = odometry.size(); vector<RangeTriple> triples = readTriples(); size_t K = triples.size(); // parameters size_t minK = 150; // minimum number of range measurements to process initially size_t incK = 25; // minimum number of range measurements to process after bool groundTruth = false; bool robust = 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)); Values initial; initial.insert(0, pose0); // initialize points if (groundTruth) { // from TL file 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)); } else { // drawn from sigma=1 Gaussian in matlab version initial.insert(symbol('L', 1), Point2(3.5784, 2.76944)); initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492)); initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549)); initial.insert(symbol('L', 5), Point2(0.714743, -0.204966)); } // set some loop variables size_t i = 1; // step counter size_t k = 0; // range measurement counter bool initialized = false; Pose2 lastPose = pose0; size_t countK = 0; // Loop over odometry gttic_(iSAM); BOOST_FOREACH(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); // 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]); newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise)); k = k + 1; countK = countK + 1; } // Check whether to update iSAM 2 if ((k > minK) && (countK > incK)) { if (!initialized) { // Do a full optimize for first minK ranges gttic_(batchInitialization); LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial); initial = batchOptimizer.optimize(); gttoc_(batchInitialization); initialized = true; } gttic_(update); isam.update(newFactors, initial); gttoc_(update); gttic_(calculateEstimate); Values result = isam.calculateEstimate(); gttoc_(calculateEstimate); lastPose = result.at<Pose2>(i); newFactors = NonlinearFactorGraph(); initial = Values(); countK = 0; } i += 1; //--------------------------------- odometry loop ----------------------------------------- } // BOOST_FOREACH
/* ************************************************************************* */ Pose2 Pose2betweenDefault(const Pose2& r1, const Pose2& r2) { return r1.inverse() * r2; }
karto::String StringHelper::ToString(const Pose2& rValue) { return rValue.ToString(); }