def add_point(self, pointsInitial, measurements, octave): if pointsInitial[-1] > self.depth_threshold: information = self.inv_lvl_sigma2[octave] * np.identity(2) stereo_model = gt.noiseModel_Diagonal.Information(information) huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaMono) robust_model = gt.noiseModel_Robust(huber, stereo_model) factor = gt.GenericProjectionFactorCal3_S2( gt.Point2(measurements[0], measurements[2]), robust_model, X(1), L(self.counter), self.K_mono) self.is_stereo.append(False) else: information = self.inv_lvl_sigma2[octave] * np.identity(3) stereo_model = gt.noiseModel_Diagonal.Information(information) huber = gt.noiseModel_mEstimator_Huber.Create(self.deltaStereo) robust_model = gt.noiseModel_Robust(huber, stereo_model) factor = gt.GenericStereoFactor3D( gt.StereoPoint2(*tuple(measurements)), robust_model, X(1), L(self.counter), self.K_stereo) self.is_stereo.append(True) self.graph.add( gt.NonlinearEqualityPoint3(L(self.counter), gt.Point3(pointsInitial))) self.initialEstimate.insert(L(self.counter), gt.Point3(pointsInitial)) self.graph.add(factor) self.octave.append(octave) self.counter += 1
def test_point3(self): """Test for Point3 version.""" factor = gtsam.NonlinearEquality2Point3(0, 1) error = factor.evaluateError(gtsam.Point3(0, 0, 0), gtsam.Point3(0, 0, 0)) np.testing.assert_allclose(error, np.zeros(3))
def sim3_transform_map(result, nrCameras, nrPoints): # Similarity transform s_poses = [] s_points = [] _sim3 = sim3.Similarity3() for i in range(nrCameras): pose_i = result.atPose3(symbol(ord('x'), i)) s_poses.append(pose_i) for j in range(nrPoints): point_j = result.atPoint3(symbol(ord('p'), j)) s_points.append(point_j) theta = np.radians(30) wRc = gtsam.Rot3(np.array([[-math.sin(theta), 0, math.cos(theta)], [-math.cos(theta), 0, -math.sin(theta)], [0, 1, 0]])) d_pose1 = gtsam.Pose3( wRc, gtsam.Point3(-math.sin(theta)*1.58, -math.cos(theta)*1.58, 1.2)) d_pose2 = gtsam.Pose3( wRc, gtsam.Point3(-math.sin(theta)*1.58*3, -math.cos(theta)*1.58*3, 1.2)) pose_pairs = [[s_poses[2], d_pose2], [s_poses[0], d_pose1]] _sim3.align_pose(pose_pairs) print('R', _sim3._R) print('t', _sim3._t) print('s', _sim3._s) s_map = (s_poses, s_points) actual_poses, actual_points = _sim3.map_transform(s_map) d_map = _sim3.map_transform(s_map) return _sim3, d_map
def n2g(numpy_arr, obj): if obj == "Quaternion": x, y, z, w = numpy_arr return gtsam.Rot3.Quaternion(w, x, y, z) elif obj == "Euler": roll, pitch, yaw = numpy_arr return gtsam.Rot3.Ypr(yaw, pitch, roll) elif obj == "Point2": x, y = numpy_arr return gtsam.Point2(x, y) elif obj == "Pose2": x, y, yaw = numpy_arr return gtsam.Pose2(x, y, yaw) elif obj == "Point3": x, y, z = numpy_arr return gtsam.Point3(x, y, z) elif obj == "Pose3": x, y, z, roll, pitch, yaw = numpy_arr return gtsam.Pose3(gtsam.Rot3.Ypr(yaw, pitch, roll), gtsam.Point3(x, y, z)) elif obj == "imuBiasConstantBias": imu_bias = numpy_arr return gtsam.imuBias_ConstantBias(np.array(imu_bias[:3]), np.array(imu_bias[3:])) elif obj == "Vector": return np.array(numpy_arr) else: raise NotImplementedError("Not implemented from numpy to " + obj)
def estimate_transform(clouda, cloudb): """Estimate the transform from clouda to cloudb. Returns a gtsam.Pose3 object. Args: clouda (ndarray): point cloud A cloudb (ndarray): point cloud B """ if clouda.shape != cloudb.shape and clouda.shape[1] < 3: return None centroida = np.average(clouda, axis=1) centroidb = np.average(cloudb, axis=1) clouda_prime = clouda - centroida[:, np.newaxis] cloudb_prime = cloudb - centroidb[:, np.newaxis] H = np.sum(clouda_prime.T[:, :, None] * cloudb_prime.T[:, None], axis=0) aRb = gtsam.Rot3.ClosestTo(H) rot_centroidb = aRb.rotate(gtsam.Point3(centroidb)) atb = gtsam.Point3( centroida - np.array([rot_centroidb.x(), rot_centroidb.y(), rot_centroidb.z()])) return gtsam.Pose3(aRb, atb).inverse()
def ExampleValues(): T = [] T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65]))) T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00]))) T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00]))) data = gtsam.Values() for i in range(len(T)): data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i])) return data
def createPoses(): # Create the set of ground-truth poses radius = 30.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), 0.0) camera = gtsam.PinholeCameraCal3_S2.Lookat(position, target, up) poses.append(camera.pose()) return poses
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()) return poses
def setUp(self): """Set up two constraints to test.""" aTb = gtsam.Pose3(gtsam.Rot3.Yaw(np.pi / 2), gtsam.Point3(1, 2, 3)) cov = gtsam.noiseModel.Isotropic.Variance(6, 1e-3).covariance() counts = np.zeros((5, 5)) counts[0][0] = 200 self.a_constraint_b = Constraint(1, 2, aTb, cov, counts) aTc = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.5, 0.5, 0.5)) variances = np.array([1e-4, 1e-5, 1e-6, 1e-1, 1e-2, 1e-3]) aCOVc = gtsam.noiseModel.Diagonal.Variances(variances).covariance() counts = np.zeros((5, 5)) counts[1][2] = 100 self.a_constraint_c = Constraint(1, 3, aTc, aCOVc, counts)
def motion_sample(self, action, action_noise_matrix, ML=False): # Taking the path length for using the latest pose try: self.optimize() except: pass path_length = len(self.daRealization) new_pose = self.result.atPose3(self.X(path_length)).compose(action) new_pose_vector = [ new_pose.rotation().rpy()[0], new_pose.rotation().rpy()[1], new_pose.rotation().rpy()[2], new_pose.x(), new_pose.y(), new_pose.z() ] # Sample unless ML assumption is true if ML is False: sampled_new_pose_vector = np.random.multivariate_normal( new_pose_vector, action_noise_matrix) return gtsam.Pose3( gtsam.Rot3.Ypr(sampled_new_pose_vector[2], sampled_new_pose_vector[1], sampled_new_pose_vector[0]), gtsam.Point3(sampled_new_pose_vector[3], sampled_new_pose_vector[4], sampled_new_pose_vector[5])) else: return new_pose
def setUp(self): self.testclouda = np.array([[1], [1], [1]]) self.testcloudb = np.array([[2, 10], [1, 1], [1, 1]]) self.testcloudc = np.array([[2], [1], [1]]) self.testbTa = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)) self.testcloudd = np.array([[0, 20, 10], [0, 10, 20], [0, 0, 0]]) self.testcloude = np.array([[10, 30, 20], [10, 20, 30], [0, 0, 0]])
def main(): """Main runner.""" # Create an empty nonlinear factor graph graph = gtsam.NonlinearFactorGraph() # Add a prior on the first point, setting it to the origin # A prior factor consists of a mean and a noise model (covariance matrix) priorMean = gtsam.Pose3() # prior at origin graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE)) # Add GPS factors gps = gtsam.Point3(lat0, lon0, h0) graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE)) print("\nFactor Graph:\n{}".format(graph)) # Create the data structure to hold the initialEstimate estimate to the solution # For illustrative purposes, these have been deliberately set to incorrect values initial = gtsam.Values() initial.insert(1, gtsam.Pose3()) print("\nInitial Estimate:\n{}".format(initial)) # optimize using Levenberg-Marquardt optimization params = gtsam.LevenbergMarquardtParams() optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) result = optimizer.optimize() print("\nFinal Result:\n{}".format(result))
def circlePose3(numPoses=8, radius=1.0, symbolChar='\0'): """ circlePose3 generates a set of poses in a circle. This function returns those poses inside a gtsam.Values object, with sequential keys starting from 0. An optional character may be provided, which will be stored in the msb of each key (i.e. gtsam.Symbol). We use aerospace/navlab convention, X forward, Y right, Z down First pose will be at (R,0,0) ^y ^ X | | z-->xZ--> Y (z pointing towards viewer, Z pointing away from viewer) Vehicle at p0 is looking towards y axis (X-axis points towards world y) """ values = gtsam.Values() theta = 0.0 dtheta = 2 * pi / numPoses gRo = gtsam.Rot3( np.array([[0., 1., 0.], [1., 0., 0.], [0., 0., -1.]], order='F')) for i in range(numPoses): key = gtsam.symbol(symbolChar, i) gti = gtsam.Point3(radius * cos(theta), radius * sin(theta), 0) oRi = gtsam.Rot3.Yaw( -theta) # negative yaw goes counterclockwise, with Z down ! gTi = gtsam.Pose3(gRo.compose(oRi), gti) values.insert(key, gTi) theta = theta + dtheta return values
def __init__(self, points, quadrics, n_interpolate=50): """ :param - points: list of camera positions (Point3) :param - quadrics: list of ConstrainedDualQuadrics :param - n_interpolate: number of points to be generated between 'points'. """ # generate camera poses looking at a quadric target = quadrics[0].pose().translation() poses = [ gtsam.SimpleCamera.Lookat(point, target, gtsam.Point3(0, 0, 1)).pose() for point in points ] # interpolate poses into trajectory self.true_trajectory = self.interpolate_poses(poses, n_interpolate) # create quadrics from list self.true_quadrics = Quadrics(dict(zip(range(len(quadrics)), quadrics))) # create box measurements self.true_boxes = self.reproject_quadrics(self.true_quadrics, self.true_trajectory)
def test_extractPoint3(self): """Test extractPoint3.""" initial = gtsam.Values() point3 = gtsam.Point3(0.0, 0.1, 0.0) initial.insert(1, gtsam.Pose2(0.0, 0.1, 0.1)) initial.insert(2, point3) np.testing.assert_equal(gtsam.utilities.extractPoint3(initial), point3.reshape(-1, 3))
def test_perturbPoint3(self): """Test perturbPoint3.""" values = gtsam.Values() point3 = gtsam.Point3(0, 0, 0) values.insert(0, gtsam.Pose2()) values.insert(1, point3) gtsam.utilities.perturbPoint3(values, 1) self.assertTrue(not np.allclose(values.atPoint3(1), point3))
def add_mr_object_measurement(self, stack, class_realization, new_input_object_prior=None, new_input_object_covariance=None): graph_add = gtsam.NonlinearFactorGraph() initial_add = gtsam.Values() idx = 0 for obj in stack: cov_noise_model = stack[obj]['covariance'] exp_gtsam = gtsam.Pose3( gtsam.Rot3.Ypr(stack[obj]['expectation'][2], stack[obj]['expectation'][1], stack[obj]['expectation'][0]), gtsam.Point3(stack[obj]['expectation'][3], stack[obj]['expectation'][4], stack[obj]['expectation'][5])) pose_rotation_matrix = exp_gtsam.rotation().matrix() cov_noise_model_rotated = self.rotate_cov_6x6( cov_noise_model, np.transpose(pose_rotation_matrix)) cov_noise_model_rotated = gtsam.noiseModel.Gaussian.Covariance( cov_noise_model_rotated) #updating the graph self.graph.add( gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam, cov_noise_model_rotated)) graph_add.add( gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam, cov_noise_model_rotated)) self.prop_belief.add( gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam, cov_noise_model_rotated)) if self.prop_initial.exists(self.XO(obj)) is False: self.prop_initial.insert( self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0))) # if obj in new_input_object_prior and obj not in self.classRealization: # self.graph.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj], # new_input_object_covariance[obj])) # self.prop_belief.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj], # new_input_object_covariance[obj])) self.classRealization[obj] = class_realization[obj] if self.isam.value_exists(self.XO(obj)) is False: initial_add.insert(self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0))) idx += 1 if stack[obj]['weight'] > -322: self.logWeight += stack[obj]['weight'] # + 150 else: self.logWeight = -math.inf
def test_allPose3s(self): """Test allPose3s.""" initial = gtsam.Values() initial.insert(0, gtsam.Pose3()) initial.insert(2, gtsam.Point2(1, 1)) initial.insert(1, gtsam.Pose3()) initial.insert(3, gtsam.Point3(1, 2, 3)) result = gtsam.utilities.allPose3s(initial) self.assertEqual(result.size(), 2)
def test_insertBackprojections(self): """Test insertBackprojections.""" values = gtsam.Values() cam = gtsam.PinholeCameraCal3_S2() gtsam.utilities.insertBackprojections( values, cam, [0, 1, 2], np.asarray([[20, 30, 40], [20, 30, 40]]), 10) np.testing.assert_allclose(values.atPoint3(0), gtsam.Point3(200, 200, 10))
def create_past_poses(self, delta): """Create the set of ground-truth poses of the last time step.""" for i, y in enumerate([0, 2.5, 5, 7.5, 10]): wRc = gtsam.Rot3( np.array([[0 + delta, 1 + delta, 0 + delta], [0 + delta, 0 - delta, -1 + delta], [1 - delta, 0 + delta, 0 - delta]]).T) self.past_poses.append( gtsam.Pose3(wRc, gtsam.Point3(0, y - delta, 1.5)))
def test_extractPose2(self): """Test extractPose2.""" initial = gtsam.Values() pose2 = np.asarray((0.0, 0.1, 0.1)) initial.insert(1, gtsam.Pose2(*pose2)) initial.insert(2, gtsam.Point3(0.0, 0.1, 0.0)) np.testing.assert_allclose(gtsam.utilities.extractPose2(initial), pose2.reshape(-1, 3))
def add_LandmarkEstimate(self, L_id, Pt_estimate): if Pt_estimate.ndim == 1: raise ValueError( "Supplied point is 1-dimensional, required Nx3 array") if Pt_estimate.shape[1] != 3: raise ValueError( "2nd dimension on supplied point is not 3, required Nx3 array") for l, p_est in zip(L_id, Pt_estimate): self.initial_estimate.insert(iSAM2Wrapper.get_key('l', l), gtsam.Point3(*p_est))
def createPoints(): # Create the set of ground-truth landmarks points = [ gtsam.Point3(10.0, 10.0, 10.0), gtsam.Point3(-10.0, 10.0, 10.0), gtsam.Point3(-10.0, -10.0, 10.0), gtsam.Point3(10.0, -10.0, 10.0), gtsam.Point3(10.0, 10.0, -10.0), gtsam.Point3(-10.0, 10.0, -10.0), gtsam.Point3(-10.0, -10.0, -10.0), gtsam.Point3(10.0, -10.0, -10.0) ] return points
def setUp(cls): test_clouds = read_ply(*scans_fnames)[:6] PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) test_graph = gtsam.NonlinearFactorGraph() test_initial_estimates = gtsam.Values() initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701), gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633)) test_graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE)) test_initial_estimates.insert(0, initial_pose) test_graph, test_initial_estimates = populate_factor_graph(test_graph, test_initial_estimates, initial_pose, test_clouds) cls.graph = test_graph cls.initial_estimates = test_initial_estimates
def __init__(self, body_ptx: float, body_pty: float, body_ptz: float, body_prx: float, body_pry: float, body_prz: float, accelerometer_sigma: float, gyroscope_sigma: float, integration_sigma: float, accelerometer_bias_sigma: float, gyroscope_bias_sigma: float, average_delta_t: float): self.bodyTimu = Pose3(gtsam.Rot3.RzRyRx(body_prx, body_pry, body_prz), gtsam.Point3(body_ptx, body_pty, body_ptz)) self.accelerometer_sigma = accelerometer_sigma self.gyroscope_sigma = gyroscope_sigma self.integration_sigma = integration_sigma self.accelerometer_bias_sigma = accelerometer_bias_sigma self.gyroscope_bias_sigma = gyroscope_bias_sigma self.average_delta_t = average_delta_t
def horn_align(trajectory1, trajectory2): """ Aligns trajectory1 with trajectory2 using HORN """ xyz1 = np.matrix([ p.translation().vector() for p in trajectory1.data() ]).transpose() xyz2 = np.matrix([ p.translation().vector() for p in trajectory2.data() ]).transpose() R, T, trans_error = Evaluation._horn_align(xyz1, xyz2) transform = gtsam.Pose3(gtsam.Rot3(R), gtsam.Point3( np.array(T)[:, 0])) # T = [[x],[y],[z]] [:,0] = tranpose()[0] warped_trajectory = trajectory1.applyTransform(transform) # print('trans_error: ', np.sqrt(np.dot(trans_error,trans_error) / len(trans_error))) return warped_trajectory
def test_insertProjectionFactors(self): """Test insertProjectionFactors.""" graph = gtsam.NonlinearFactorGraph() gtsam.utilities.insertProjectionFactors( graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2()) self.assertEqual(graph.size(), 2) graph = gtsam.NonlinearFactorGraph() gtsam.utilities.insertProjectionFactors( graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]), gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(), gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0))) self.assertEqual(graph.size(), 2)
def plot_cameras_gtsam(transform_matrix_filename: str): set_cams = np.load( '/home/sushmitawarrier/clevr-dataset-gen/output/images/CLEVR_new000000/set_cams.npz' ) with open(transform_matrix_filename) as f: transforms = json.load(f) figure_number = 0 fig = plt.figure(figure_number) axes = fig.gca(projection='3d') plt.cla() # Plot cameras idx = 0 for i in range(len(set_cams['set_rot'])): print(set_cams['set_rot'][i]) print("i", i) rot = set_cams['set_rot'][i] t = set_cams['set_loc'][i] rot_3 = gtsam.Rot3.RzRyRx(rot) pose_i = gtsam.Pose3(rot_3, t) if idx % 2 == 0: gtsam_plot.plot_pose3(figure_number, pose_i, 1) break idx += 1 for i in transforms['frames']: T = i['transform_matrix'] pose_i = gtsam.Pose3(T) #print("T", T) #print("pose_i",pose_i) #print(set_cams['set_loc'][0]) #print(set_cams['set_rot'][0]) # Reference pose # trying to look along x-axis, 0.2m above ground plane # x forward, y left, z up (x-y is the ground plane) upright = gtsam.Rot3.Ypr(-np.pi / 2, 0., -np.pi / 2) pose_j = gtsam.Pose3(upright, gtsam.Point3(0, 0, 0.2)) axis_length = 1 # plotting alternate poses if idx % 2 == 0: # gtsam_plot.plot_pose3(figure_number, pose_i, axis_length) gtsam_plot.plot_pose3(figure_number, pose_j, axis_length) idx += 1 x_axe, y_axe, z_axe = 10, 10, 10 # Draw axes.set_xlim3d(-x_axe, x_axe) axes.set_ylim3d(-y_axe, y_axe) axes.set_zlim3d(0, z_axe) plt.legend() plt.show()
def test_sfm_factor2(self): """Evaluate residual with camera, pose and point as state variables""" graph = gtsam.NonlinearFactorGraph() state = gtsam.Values() measured = self.img_point noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1) camera_key, pose_key, landmark_key = K(0), P(0), L(0) k = gtsam.Cal3Fisheye() state.insert_cal3fisheye(camera_key, k) state.insert_pose3(pose_key, gtsam.Pose3()) state.insert_point3(landmark_key, gtsam.Point3(self.obj_point)) factor = gtsam.GeneralSFMFactor2Cal3Fisheye(measured, noise_model, pose_key, landmark_key, camera_key) graph.add(factor) score = graph.error(state) self.assertAlmostEqual(score, 0)
def bounds_3D(self, box3D, pose, calibration, color=(255, 0, 255)): # convert 3D box to set of 8 points b = box3D.vector() points_3D = np.array([[x, y, z] for x in b[0:2] for y in b[2:4] for z in b[4:6]]) # ensure points infront of camera for point in points_3D: if pose.between(gtsam.Pose3( gtsam.Rot3(), gtsam.Point3(point))).translation().vector()[-1] < 0: return # project 3D points to 2D P = quadricslam.QuadricCamera.transformToImage(pose, calibration) points_3DTH = np.concatenate( (points_3D.T, np.ones((1, points_3D.shape[0])))) points_2D = P.dot(points_3DTH) points_2D = points_2D[0:2, :] / points_2D[2] points_2D = points_2D.transpose() # only draw if all points project correctly if np.any(np.isinf(points_2D)) or np.any(np.isnan(points_2D)): return # draw points for point in points_2D: cv2.circle(self._image, tuple(point.astype('int')), 1, color=(0, 0, 255), thickness=-1, lineType=cv2.LINE_AA) # draw lines for index in [(0, 1), (1, 3), (3, 2), (2, 0), (4, 5), (5, 7), (7, 6), (6, 4), (2, 6), (1, 5), (0, 4), (3, 7)]: p1 = tuple(points_2D[index[0]].astype('int')) p2 = tuple(points_2D[index[1]].astype('int')) cv2.line(self._image, p1, p2, color=color, thickness=1, lineType=cv2.LINE_AA)