rename variables
							parent
							
								
									67ffd1e1ae
								
							
						
					
					
						commit
						c0321443bd
					
				|  | @ -49,7 +49,7 @@ class Experiment { | |||
| 
 | ||||
|   // false: run original iSAM2 without ambiguities
 | ||||
|   // true: run original iSAM2 with ambiguities
 | ||||
|   const bool is_with_ambiguity = false; | ||||
|   const bool isWithAmbiguity = false; | ||||
| 
 | ||||
|  private: | ||||
|   ISAM2Params parameters; | ||||
|  | @ -69,7 +69,7 @@ class Experiment { | |||
|   /// @brief Run the main experiment with a given maxLoopCount.
 | ||||
|   void run() { | ||||
|     // Initialize local variables
 | ||||
|     size_t pose_count = 0, index = 0; | ||||
|     size_t poseCount = 0, index = 0; | ||||
| 
 | ||||
|     std::list<double> timeList; | ||||
| 
 | ||||
|  | @ -77,7 +77,7 @@ class Experiment { | |||
|     Pose2 priorPose(0, 0, 0); | ||||
|     initial_.insert(X(0), priorPose); | ||||
|     graph.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel); | ||||
|     pose_count++; | ||||
|     poseCount++; | ||||
| 
 | ||||
|     // Initial update
 | ||||
|     isam2.update(graph, initial_); | ||||
|  | @ -88,7 +88,7 @@ class Experiment { | |||
|     // Start main loop
 | ||||
|     size_t keyS = 0; | ||||
|     size_t keyT = 0; | ||||
|     clock_t start_time = clock(); | ||||
|     clock_t startTime = clock(); | ||||
| 
 | ||||
|     std::vector<Pose2> poseArray; | ||||
|     std::pair<size_t, size_t> keys; | ||||
|  | @ -98,34 +98,35 @@ class Experiment { | |||
|       keyT = keys.second; | ||||
|       size_t numMeasurements = poseArray.size(); | ||||
| 
 | ||||
|       Pose2 odom_pose; | ||||
|       if (is_with_ambiguity) { | ||||
|       Pose2 odomPose; | ||||
|       if (isWithAmbiguity) { | ||||
|         // Get wrong intentionally
 | ||||
|         int id = index % num_measurements; | ||||
|         odom_pose = Pose2(pose_array[id]); | ||||
|         int id = index % numMeasurements; | ||||
|         odomPose = Pose2(poseArray[id]); | ||||
|       } else { | ||||
|         odom_pose = pose_array[0]; | ||||
|         odomPose = poseArray[0]; | ||||
|       } | ||||
| 
 | ||||
|       if (keyS == keyT - 1) {  // new X(key)
 | ||||
|         initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odom_pose); | ||||
|         initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odomPose); | ||||
|         graph.add( | ||||
|             BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, kPoseNoiseModel)); | ||||
|         pose_count++; | ||||
|             BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel)); | ||||
|         poseCount++; | ||||
| 
 | ||||
|       } else {  // loop
 | ||||
|         int id = index % num_measurements; | ||||
|         if (is_with_ambiguity && id % 2 == 0) { | ||||
|           graph.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, | ||||
|                                           kPoseNoiseModel)); | ||||
|         int id = index % numMeasurements; | ||||
|         if (isWithAmbiguity && id % 2 == 0) { | ||||
|           graph.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, | ||||
|                                          kPoseNoiseModel)); | ||||
|         } else { | ||||
|           graph.add(BetweenFactor<Pose2>( | ||||
|               X(keyS), X(keyT), odom_pose, | ||||
|               X(keyS), X(keyT), odomPose, | ||||
|               noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0))); | ||||
|         } | ||||
|         index++; | ||||
|       } | ||||
| 
 | ||||
|       isam2.update(*graph, initial_); | ||||
|       isam2.update(graph, initial_); | ||||
|       graph.resize(0); | ||||
|       initial_.clear(); | ||||
|       results = isam2.calculateBestEstimate(); | ||||
|  | @ -133,45 +134,45 @@ class Experiment { | |||
|       // Print loop index and time taken in processor clock ticks
 | ||||
|       if (index % 50 == 0 && keyS != keyT - 1) { | ||||
|         std::cout << "index: " << index << std::endl; | ||||
|         std::cout << "acc_time:  " << timeList.back() / CLOCKS_PER_SEC | ||||
|         std::cout << "accTime:  " << timeList.back() / CLOCKS_PER_SEC | ||||
|                   << std::endl; | ||||
|       } | ||||
| 
 | ||||
|       if (keyS == keyT - 1) { | ||||
|         clock_t cur_time = clock(); | ||||
|         timeList.push_back(cur_time - start_time); | ||||
|         clock_t curTime = clock(); | ||||
|         timeList.push_back(curTime - startTime); | ||||
|       } | ||||
| 
 | ||||
|       if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) { | ||||
|         string step_file_idx = std::to_string(100000 + timeList.size()); | ||||
|         string stepFileIdx = std::to_string(100000 + timeList.size()); | ||||
| 
 | ||||
|         ofstream step_outfile; | ||||
|         string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx; | ||||
|         step_outfile.open(step_file_name + ".txt"); | ||||
|         ofstream stepOutfile; | ||||
|         string stepFileName = "step_files/ISAM2_city10000_S" + stepFileIdx; | ||||
|         stepOutfile.open(stepFileName + ".txt"); | ||||
|         for (size_t i = 0; i < (keyT + 1); ++i) { | ||||
|           Pose2 out_pose = results.at<Pose2>(X(i)); | ||||
|           step_outfile << out_pose.x() << " " << out_pose.y() << " " | ||||
|                        << out_pose.theta() << endl; | ||||
|           Pose2 outPose = results.at<Pose2>(X(i)); | ||||
|           stepOutfile << outPose.x() << " " << outPose.y() << " " | ||||
|                       << outPose.theta() << endl; | ||||
|         } | ||||
|         step_outfile.close(); | ||||
|         stepOutfile.close(); | ||||
|       } | ||||
|     } | ||||
| 
 | ||||
|     clock_t end_time = clock(); | ||||
|     clock_t total_time = end_time - start_time; | ||||
|     cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl; | ||||
|     clock_t endTime = clock(); | ||||
|     clock_t totalTime = endTime - startTime; | ||||
|     cout << "totalTime: " << totalTime / CLOCKS_PER_SEC << endl; | ||||
| 
 | ||||
|     /// Write results to file
 | ||||
|     writeResult(results, (keyT + 1), "ISAM2_city10000.txt"); | ||||
| 
 | ||||
|     ofstream outfile_time; | ||||
|     std::string time_file_name = "ISAM2_city10000_time.txt"; | ||||
|     outfile_time.open(time_file_name); | ||||
|     for (auto acc_time : timeList) { | ||||
|       outfile_time << acc_time << std::endl; | ||||
|     ofstream outfileTime; | ||||
|     std::string timeFileName = "ISAM2_city10000_time.txt"; | ||||
|     outfileTime.open(timeFileName); | ||||
|     for (auto accTime : timeList) { | ||||
|       outfileTime << accTime << std::endl; | ||||
|     } | ||||
|     outfile_time.close(); | ||||
|     cout << "Written cumulative time to: " << time_file_name << " file." | ||||
|     outfileTime.close(); | ||||
|     cout << "Written cumulative time to: " << timeFileName << " file." | ||||
|          << endl; | ||||
|   } | ||||
| }; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue