/* ************************************************************************* */ TEST( dataSet, writeBALfromValues_Dubrovnik){ ///< Read a file using the unit tested readBAL const string filenameToRead = findExampleDataFile("dubrovnik-3-7-pre"); SfM_data readData; readBAL(filenameToRead, readData); Pose3 poseChange = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.3,0.1,0.3)); Values value; for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera Key poseKey = symbol('x',i); Pose3 pose = poseChange.compose(readData.cameras[i].pose()); value.insert(poseKey, pose); } for(size_t j=0; j < readData.number_tracks(); j++){ // for each point Key pointKey = P(j); Point3 point = poseChange.transform_from( readData.tracks[j].p ); value.insert(pointKey, point); } // Write values and readData to a file const string filenameToWrite = createRewrittenFileName(filenameToRead); writeBALfromValues(filenameToWrite, readData, value); // Read the file we wrote SfM_data writtenData; readBAL(filenameToWrite, writtenData); // Check that the reprojection errors are the same and the poses are correct // Check number of things EXPECT_LONGS_EQUAL(3,writtenData.number_cameras()); EXPECT_LONGS_EQUAL(7,writtenData.number_tracks()); const SfM_Track& track0 = writtenData.tracks[0]; EXPECT_LONGS_EQUAL(3,track0.number_measurements()); // Check projection of a given point EXPECT_LONGS_EQUAL(0,track0.measurements[0].first); const SfM_Camera& camera0 = writtenData.cameras[0]; Point2 expected = camera0.project(track0.p), actual = track0.measurements[0].second; EXPECT(assert_equal(expected,actual,12)); Pose3 expectedPose = camera0.pose(); Key poseKey = symbol('x',0); Pose3 actualPose = value.at<Pose3>(poseKey); EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); Point3 expectedPoint = track0.p; Key pointKey = P(0); Point3 actualPoint = value.at<Point3>(pointKey); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); }
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration( const PinholeCamera<CALIBRATION>& camera) { GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION) Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.5, 0.1, 0.3)); Pose3 cameraPose = camera.pose(); Pose3 perturbedCameraPose = cameraPose.compose(noise_pose); typename gtsam::traits<CALIBRATION>::TangentVector d; d.setRandom(); d *= 0.1; CALIBRATION perturbedCalibration = camera.calibration().retract(d); return PinholeCamera<CALIBRATION>(perturbedCameraPose, perturbedCalibration); }
/* ************************************************************************* */ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, OptionalJacobian<9, 6> Dtrans) const { // Pose3 transform is just compose Matrix6 D_newpose_trans, D_newpose_pose; Pose3 newpose = trans.compose(pose(), D_newpose_trans, D_newpose_pose); // Note that we rotate the velocity Matrix3 D_newvel_R, D_newvel_v; Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v); if (Dglobal) { Dglobal->setZero(); Dglobal->topLeftCorner<6,6>() = D_newpose_pose; Dglobal->bottomRightCorner<3,3>() = D_newvel_v; } if (Dtrans) { Dtrans->setZero(); Dtrans->topLeftCorner<6,6>() = D_newpose_trans; Dtrans->bottomLeftCorner<3,3>() = D_newvel_R; } return PoseRTV(newpose, newvel); }
void StateEstimator::optimizationLoop() { ISAM2Params parameters; // parameters.relinearizeThreshold = 0.0; // Set the relin threshold to zero such that the batch estimate is recovered // parameters.relinearizeSkip = 1; // Relinearize every time gtsam::IncrementalFixedLagSmoother graph(optLag_, parameters); double startTime; sensor_msgs::ImuConstPtr lastImu; double lastImuT; int imuKey = 1; int gpsKey = 1; // first we will initialize the graph with appropriate priors NonlinearFactorGraph newFactors; Values newVariables; FixedLagSmoother::KeyTimestampMap newTimestamps; sensor_msgs::NavSatFixConstPtr fix = gpsQ_.pop(); startTime = ROS_TIME(fix); enu_.Reset(fix->latitude, fix->longitude, fix->altitude); sensor_msgs::ImuConstPtr imu = imuQ_.pop(); lastImu = imu; lastImuT = ROS_TIME(imu) - 1 / imuFreq_; Rot3 initialOrientation = Rot3::Quaternion(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z); // we set out initial position to the origin and assume we are stationary Pose3 x0(initialOrientation, Point3(0, 0, 0)); PriorFactor<Pose3> priorPose(X(0), x0, noiseModel::Diagonal::Sigmas((Vector(6) << priorOSigma_, priorOSigma_, priorOSigma_, priorPSigma_, priorPSigma_, priorPSigma_) .finished())); newFactors.add(priorPose); Vector3 v0 = Vector3(0, 0, 0); PriorFactor<Vector3> priorVel( V(0), v0, noiseModel::Diagonal::Sigmas((Vector(3) << priorVSigma_, priorVSigma_, priorVSigma_).finished())); newFactors.add(priorVel); imuBias::ConstantBias b0((Vector(6) << 0, 0, 0, 0, 0, 0).finished()); PriorFactor<imuBias::ConstantBias> priorBias( B(0), b0, noiseModel::Diagonal::Sigmas( (Vector(6) << priorABias_, priorABias_, priorABias_, priorGBias_, priorGBias_, priorGBias_).finished())); newFactors.add(priorBias); noiseModel::Diagonal::shared_ptr imuToGpsFactorNoise = noiseModel::Diagonal::Sigmas( (Vector(6) << gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_, gpsTSigma_).finished()); newFactors.add(BetweenFactor<Pose3>(X(0), G(0), imuToGps_, imuToGpsFactorNoise)); newVariables.insert(X(0), x0); newVariables.insert(V(0), v0); newVariables.insert(B(0), b0); newVariables.insert(G(0), x0.compose(imuToGps_)); newTimestamps[X(0)] = 0; newTimestamps[G(0)] = 0; newTimestamps[V(0)] = 0; newTimestamps[B(0)] = 0; graph.update(newFactors, newVariables); //, newTimestamps); Pose3 prevPose = prevPose_ = x0; Vector3 prevVel = prevVel_ = v0; imuBias::ConstantBias prevBias = prevBias_ = b0; // remove old imu messages while (!imuQ_.empty() && ROS_TIME(imuQ_.front()) < ROS_TIME(fix)) { lastImuT = ROS_TIME(lastImu); lastImu = imuQ_.pop(); } // setting up the IMU integration boost::shared_ptr<gtsam::PreintegrationParams> preintegrationParams = PreintegrationParams::MakeSharedU(gravityMagnitude_); preintegrationParams->accelerometerCovariance = accelSigma_ * I_3x3; preintegrationParams->gyroscopeCovariance = gyroSigma_ * I_3x3; preintegrationParams->integrationCovariance = imuIntSigma_ * I_3x3; PreintegratedImuMeasurements imuIntegrator(preintegrationParams, prevBias); Vector noiseModelBetweenBias = (Vector(6) << accelBSigma_, accelBSigma_, accelBSigma_, gyroBSigma_, gyroBSigma_, gyroBSigma_).finished(); SharedDiagonal gpsNoise = noiseModel::Diagonal::Sigmas(Vector3(gpsSigma_, gpsSigma_, 3 * gpsSigma_)); newFactors.resize(0); newVariables.clear(); newTimestamps.clear(); // now we loop and let use the queues to grab messages while (ros::ok()) { bool optimize = false; // integrate imu messages while (!imuQ_.empty() && ROS_TIME(imuQ_.back()) > (startTime + 0.1 * imuKey) && !optimize) { double curTime = startTime + 0.1 * imuKey; // we reset the integrator, then integrate imuIntegrator.resetIntegrationAndSetBias(prevBias); while (ROS_TIME(lastImu) < curTime) { double dt = ROS_TIME(lastImu) - lastImuT; imuIntegrator.integrateMeasurement( Vector3(lastImu->linear_acceleration.x, lastImu->linear_acceleration.y, lastImu->linear_acceleration.z), Vector3(lastImu->angular_velocity.x, lastImu->angular_velocity.y, lastImu->angular_velocity.z), dt); lastImuT = ROS_TIME(lastImu); lastImu = imuQ_.pop(); } // now put this into the graph ImuFactor imuf(X(imuKey - 1), V(imuKey - 1), X(imuKey), V(imuKey), B(imuKey - 1), imuIntegrator); newFactors.add(imuf); newFactors.add(BetweenFactor<imuBias::ConstantBias>( B(imuKey - 1), B(imuKey), imuBias::ConstantBias(), noiseModel::Diagonal::Sigmas(sqrt(imuIntegrator.deltaTij()) * noiseModelBetweenBias))); Rot3 orientation = Rot3::Quaternion(lastImu->orientation.w, lastImu->orientation.x, lastImu->orientation.y, lastImu->orientation.z); // std::cout << "adding orientation: " << orientation.xyz() << std::endl; newFactors.add(UnaryRotationFactor(X(imuKey), orientation.yaw(), noiseModel::Diagonal::Sigmas((Vector(1) << yawSigma_).finished()))); NavState cur(prevPose, prevVel); NavState next = imuIntegrator.predict(cur, prevBias); prevPose = next.pose(); prevVel = next.v(); newVariables.insert(X(imuKey), prevPose); newVariables.insert(G(imuKey), prevPose.compose(imuToGps_)); newVariables.insert(V(imuKey), prevVel); newVariables.insert(B(imuKey), prevBias); Pose3 temp = prevPose.compose(imuToGps_); std::cout << "imu(" << imuKey << "): " << temp.x() << " " << temp.y() << " " << temp.z() << std::endl; // for marginalizing out past the time window newTimestamps[X(imuKey)] = 0.1 * imuKey; newTimestamps[G(imuKey)] = 0.1 * imuKey; newTimestamps[V(imuKey)] = 0.1 * imuKey; newTimestamps[B(imuKey)] = 0.1 * imuKey; ++imuKey; optimize = true; } while (!gpsQ_.empty() && gpsKey < imuKey && optimize && ROS_TIME(gpsQ_.back()) > (startTime + gpsKey * 0.1)) { fix = gpsQ_.pop(); // we don't want all gps messages, just ones that are very close to the factors (10 hz) if (std::abs(ROS_TIME(fix) - (startTime + gpsKey * 0.1)) > 1e-2) continue; double E, N, U; enu_.Forward(fix->latitude, fix->longitude, fix->altitude, E, N, U); // we should maybe do a check on the GPS to make sure it's valid newFactors.add(GPSFactor(G(gpsKey), Point3(E, N, U), gpsNoise)); newFactors.add(BetweenFactor<Pose3>(X(gpsKey), G(gpsKey), imuToGps_, imuToGpsFactorNoise)); std::cout << "gps(" << gpsKey << "): " << E << " " << N << " " << U << std::endl; ++gpsKey; } if (!optimize) continue; try { graph.update(newFactors, newVariables); //, newTimestamps); prevPose = graph.calculateEstimate<Pose3>(X(imuKey - 1)); prevVel = graph.calculateEstimate<Vector3>(V(imuKey - 1)); prevBias = graph.calculateEstimate<imuBias::ConstantBias>(B(imuKey - 1)); // pass this to the other thread { std::lock_guard<std::mutex> lock(mutex_); prevPose_ = prevPose; prevVel_ = prevVel; prevBias_ = prevBias; currentTime_ = (imuKey - 1) * 0.1 + startTime; doneFirstOpt_ = true; } } catch (IndeterminantLinearSystemException ex) { // optimization blew up, not much to do just warn user ROS_ERROR("Indeterminant linear system error"); } newFactors.resize(0); newVariables.clear(); newTimestamps.clear(); } }