Fix createPoses
							parent
							
								
									d48b1fc840
								
							
						
					
					
						commit
						26a3728d80
					
				|  | @ -56,7 +56,7 @@ std::vector<Point3> createPoints() { | |||
| 
 | ||||
| /**
 | ||||
|  * Create a set of ground-truth poses | ||||
|  *  Default values give a circular trajectory, radius 30 at pi/4 intervals, | ||||
|  * Default values give a circular trajectory, radius 30 at pi/4 intervals, | ||||
|  * always facing the circle center | ||||
|  */ | ||||
| std::vector<Pose3> createPoses( | ||||
|  |  | |||
|  | @ -62,7 +62,7 @@ def main(): | |||
|     points = SFMdata.createPoints() | ||||
| 
 | ||||
|     # Create the set of ground-truth poses | ||||
|     poses = SFMdata.createPoses(K) | ||||
|     poses = SFMdata.createPoses() | ||||
| 
 | ||||
|     # Create a factor graph | ||||
|     graph = NonlinearFactorGraph() | ||||
|  |  | |||
|  | @ -3,14 +3,14 @@ A structure-from-motion example with landmarks | |||
|  - The landmarks form a 10 meter cube | ||||
|  - The robot rotates around the landmarks, always facing towards the cube | ||||
| """ | ||||
| 
 | ||||
| # pylint: disable=invalid-name, E1101 | ||||
| 
 | ||||
| from typing import List | ||||
| 
 | ||||
| import numpy as np | ||||
| 
 | ||||
| import gtsam | ||||
| from gtsam import Cal3_S2, Point3, Pose3 | ||||
| from gtsam import Point3, Pose3, Rot3 | ||||
| 
 | ||||
| 
 | ||||
| def createPoints() -> List[Point3]: | ||||
|  | @ -28,16 +28,49 @@ def createPoints() -> List[Point3]: | |||
|     return points | ||||
| 
 | ||||
| 
 | ||||
| def createPoses(K: Cal3_S2) -> List[Pose3]: | ||||
|     """Generate a set of ground-truth camera poses arranged in a circle about the origin.""" | ||||
|     radius = 40.0 | ||||
|     height = 10.0 | ||||
|     angles = np.linspace(0, 2 * np.pi, 8, endpoint=False) | ||||
|     up = gtsam.Point3(0, 0, 1) | ||||
|     target = gtsam.Point3(0, 0, 0) | ||||
|     poses = [] | ||||
|     for theta in angles: | ||||
|         position = gtsam.Point3(radius * np.cos(theta), radius * np.sin(theta), height) | ||||
|         camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up, K) | ||||
|         poses.append(camera.pose()) | ||||
| _M_PI_2 = np.pi / 2 | ||||
| _M_PI_4 = np.pi / 4 | ||||
| 
 | ||||
| 
 | ||||
| def createPoses( | ||||
|     init: Pose3 = Pose3(Rot3.Ypr(_M_PI_2, 0, -_M_PI_2), Point3(30, 0, 0)), | ||||
|     delta: Pose3 = Pose3( | ||||
|         Rot3.Ypr(0, -_M_PI_4, 0), | ||||
|         Point3(np.sin(_M_PI_4) * 30, 0, 30 * (1 - np.sin(_M_PI_4))), | ||||
|     ), | ||||
|     steps: int = 8, | ||||
| ) -> List[Pose3]: | ||||
|     """ | ||||
|     Create a set of ground-truth poses | ||||
|     Default values give a circular trajectory, radius 30 at pi/4 intervals, | ||||
|     always facing the circle center | ||||
|     """ | ||||
|     poses = [init] | ||||
|     for _ in range(1, steps): | ||||
|         poses.append(poses[-1].compose(delta)) | ||||
|     return poses | ||||
| 
 | ||||
| 
 | ||||
| def posesOnCircle(num_cameras=8, R=30): | ||||
|     """Create regularly spaced poses with specified radius and number of cameras.""" | ||||
|     theta = 2 * np.pi / num_cameras | ||||
| 
 | ||||
|     # Initial pose at angle 0, position (R, 0, 0), facing the center with Y-axis pointing down | ||||
|     init_rotation = Rot3.Ypr(_M_PI_2, 0, -_M_PI_2) | ||||
|     init_position = np.array([R, 0, 0]) | ||||
|     init = Pose3(init_rotation, init_position) | ||||
| 
 | ||||
|     # Delta rotation: rotate by -theta around Z-axis (counterclockwise movement) | ||||
|     delta_rotation = Rot3.Ypr(0, -theta, 0) | ||||
| 
 | ||||
|     # Delta translation in world frame | ||||
|     delta_translation_world = np.array([R * (np.cos(theta) - 1), R * np.sin(theta), 0]) | ||||
| 
 | ||||
|     # Transform delta translation to local frame of the camera | ||||
|     delta_translation_local = init.rotation().unrotate(delta_translation_world) | ||||
| 
 | ||||
|     # Define delta pose | ||||
|     delta = Pose3(delta_rotation, delta_translation_local) | ||||
| 
 | ||||
|     # Generate poses | ||||
|     return createPoses(init, delta, num_cameras) | ||||
|  |  | |||
|  | @ -31,8 +31,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]: | |||
|     that lie on a circle and face the center. The poses of 8 cameras are obtained from SFMdata | ||||
|     and the unit translations directions between some camera pairs are computed from their | ||||
|     global translations. """ | ||||
|     fx, fy, s, u0, v0 = 50.0, 50.0, 0.0, 50.0, 50.0 | ||||
|     wTc_list = SFMdata.createPoses(gtsam.Cal3_S2(fx, fy, s, u0, v0)) | ||||
|     wTc_list = SFMdata.createPoses() | ||||
|     # Rotations of the cameras in the world frame. | ||||
|     wRc_values = gtsam.Values() | ||||
|     # Normalized translation directions from camera i to camera j | ||||
|  |  | |||
|  | @ -73,7 +73,7 @@ def visual_ISAM2_example(): | |||
|     points = SFMdata.createPoints() | ||||
| 
 | ||||
|     # Create the set of ground-truth poses | ||||
|     poses = SFMdata.createPoses(K) | ||||
|     poses = SFMdata.createPoses() | ||||
| 
 | ||||
|     # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps | ||||
|     # to maintain proper linearization and efficient variable ordering, iSAM2 | ||||
|  |  | |||
|  | @ -36,7 +36,7 @@ def main(): | |||
|     # Create the set of ground-truth landmarks | ||||
|     points = SFMdata.createPoints() | ||||
|     # Create the set of ground-truth poses | ||||
|     poses = SFMdata.createPoses(K) | ||||
|     poses = SFMdata.createPoses() | ||||
| 
 | ||||
|     # Create a NonlinearISAM object which will relinearize and reorder the variables | ||||
|     # every "reorderInterval" updates | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue