Now initializing randomly
							parent
							
								
									98efc464dc
								
							
						
					
					
						commit
						f7af2c09e7
					
				|  | @ -56,10 +56,11 @@ using gtsam::Symbol; | |||
| #include <gtsam/base/timing.h> | ||||
| using gtsam::tictoc_print_; | ||||
| 
 | ||||
| 
 | ||||
| // Standard headers, added last, so we know headers above work on their own.
 | ||||
| #include <fstream> | ||||
| #include <iostream> | ||||
| #include <random> | ||||
| #include <set> | ||||
| #include <string> | ||||
| #include <utility> | ||||
| #include <vector> | ||||
|  | @ -73,7 +74,7 @@ namespace NM = gtsam::noiseModel; | |||
| 
 | ||||
| // load the odometry
 | ||||
| // DR: Odometry Input (delta distance traveled and delta heading change)
 | ||||
| //    Time (sec)  Delta Dist. Trav. (m) Delta Heading (rad)
 | ||||
| //    Time (sec)  Delta Distance Traveled (m) Delta Heading (rad)
 | ||||
| using TimedOdometry = std::pair<double, Pose2>; | ||||
| std::list<TimedOdometry> readOdometry() { | ||||
|   std::list<TimedOdometry> odometryList; | ||||
|  | @ -120,7 +121,6 @@ int main(int argc, char** argv) { | |||
|   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
 | ||||
|  | @ -129,6 +129,7 @@ int main(int argc, char** argv) { | |||
|   double sigmaR = 100;        // range standard deviation
 | ||||
|   const NM::Base::shared_ptr  // all same type
 | ||||
|       priorNoise = NM::Diagonal::Sigmas(priorSigmas),  // prior
 | ||||
|       looseNoise = NM::Isotropic::Sigma(2, 1000),      // loose LM prior
 | ||||
|       odoNoise = NM::Diagonal::Sigmas(odoSigmas),      // odometry
 | ||||
|       gaussian = NM::Isotropic::Sigma(1, sigmaR),      // non-robust
 | ||||
|       tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), | ||||
|  | @ -145,18 +146,11 @@ int main(int argc, char** argv) { | |||
|   gtsam::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)); | ||||
|   } | ||||
|   // We will initialize landmarks randomly, and keep track of which landmarks we
 | ||||
|   // already added with a set.
 | ||||
|   std::mt19937_64 rng; | ||||
|   std::normal_distribution<double> normal(0.0, 1.0); | ||||
|   std::set<Symbol> initializedLandmarks; | ||||
| 
 | ||||
|   // set some loop variables
 | ||||
|   size_t i = 1;  // step counter
 | ||||
|  | @ -185,9 +179,19 @@ int main(int argc, char** argv) { | |||
|     // 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]); | ||||
|       Symbol landmark_key('L', j); | ||||
|       double range = boost::get<2>(triples[k]); | ||||
|       newFactors.emplace_shared<gtsam::RangeFactor<Pose2, Point2>>( | ||||
|           i, Symbol('L', j), range, rangeNoise); | ||||
|           i, landmark_key, range, rangeNoise); | ||||
|       if (initializedLandmarks.count(landmark_key) == 0) { | ||||
|         double x = normal(rng), y = normal(rng); | ||||
|         initial.insert(landmark_key, Point2(x, y)); | ||||
|         initializedLandmarks.insert(landmark_key); | ||||
|         // We also add a very loose prior on the landmark in case there is only
 | ||||
|         // one sighting, which cannot fully determine the landmark.
 | ||||
|         newFactors.emplace_shared<gtsam::PriorFactor<Point2>>( | ||||
|             landmark_key, Point2(0, 0), looseNoise); | ||||
|       } | ||||
|       k = k + 1; | ||||
|       countK = countK + 1; | ||||
|     } | ||||
|  | @ -220,5 +224,12 @@ int main(int argc, char** argv) { | |||
|   // Print timings
 | ||||
|   tictoc_print_(); | ||||
| 
 | ||||
|   // Print optimized landmarks:
 | ||||
|   gtsam::Values finalResult = isam.calculateEstimate(); | ||||
|   for (auto&& landmark_key : initializedLandmarks) { | ||||
|     Point2 p = finalResult.at<Point2>(landmark_key); | ||||
|     std::cout << landmark_key << ":" << p.transpose() << "\n"; | ||||
|   } | ||||
| 
 | ||||
|   exit(0); | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue