Fixed compilation issues
							parent
							
								
									5faf09726b
								
							
						
					
					
						commit
						1c83329b9b
					
				|  | @ -412,38 +412,40 @@ TEST (Unit3, FromPoint3) { | |||
| 
 | ||||
| //*******************************************************************************
 | ||||
| TEST(Unit3, ErrorBetweenFactor) { | ||||
|   std::vector<Unit3> data = {Unit3(1.0, 0.0, 0.0), Unit3(0.0, 0.0, 1.0)}; | ||||
|   std::vector<Unit3> data; | ||||
|   data.push_back(Unit3(1.0, 0.0, 0.0)); | ||||
|   data.push_back(Unit3(0.0, 0.0, 1.0)); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   Values initial_values; | ||||
| 
 | ||||
|   // Add prior factors.
 | ||||
|   SharedNoiseModel R_prior = noiseModel::Unit::Create(2); | ||||
|   for (int i = 0; i < data.size(); i++) { | ||||
|   for (size_t i = 0; i < data.size(); i++) { | ||||
|     graph.add(PriorFactor<Unit3>(U(i), data[i], R_prior)); | ||||
|   } | ||||
| 
 | ||||
|   // Add process factors using the dot product error function.
 | ||||
|   SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01); | ||||
|   for (int i = 0; i < data.size() - 1; i++) { | ||||
|   for (size_t i = 0; i < data.size() - 1; i++) { | ||||
|     Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector, Expression<Unit3>(U(i + 1))); | ||||
|     graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp); | ||||
|   } | ||||
| 
 | ||||
|   // Add initial values. Since there is no identity, just pick something.
 | ||||
|   for (int i = 0; i < data.size(); i++) { | ||||
|   for (size_t i = 0; i < data.size(); i++) { | ||||
|     initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0)); | ||||
|   } | ||||
| 
 | ||||
|   Values values = GaussNewtonOptimizer(graph, initial_values).optimize(); | ||||
| 
 | ||||
|   // Check that the y-value is very small for each.
 | ||||
|   for (int i = 0; i < data.size(); i++) { | ||||
|   for (size_t i = 0; i < data.size(); i++) { | ||||
|     EXPECT(assert_equal(0.0, values.at<Unit3>(U(i)).unitVector().y(), 1e-3)); | ||||
|   } | ||||
| 
 | ||||
|   // Check that the dot product between variables is close to 1.
 | ||||
|   for (int i = 0; i < data.size() - 1; i++) { | ||||
|   for (size_t i = 0; i < data.size() - 1; i++) { | ||||
|     EXPECT(assert_equal(1.0, values.at<Unit3>(U(i)).dot(values.at<Unit3>(U(i + 1))), 1e-2)); | ||||
|   } | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue