//------------------------------------------------------------------------------
void ManifoldPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
deltaTij_ += dt;
deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// Update Jacobians
// TODO(frank): Try same simplification as in new approach
Matrix3 D_acc_R;
oldRij.rotate(acc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = omega * dt;
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * dt;
delVdelBiasOmega_ += D_acc_biasOmega * dt;
}
//*************************************************************************
TEST (EssentialMatrixFactor, testData) {
CHECK(readOK);
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z())
* c1Rc2.matrix();
EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
// Check some projections
EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8));
EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8));
EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8));
EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));
// Check homogeneous version
EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8));
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
}
/* ************************************************************************* */
TEST( dataSet, openGL2gtsam)
{
Vector3 rotVec(0.2, 0.7, 1.1);
Rot3 R = Rot3::Expmap(rotVec);
Point3 t = Point3(0.0,0.0,0.0);
Pose3 poseGTSAM = Pose3(R,t);
Pose3 expected = openGL2gtsam(R, t.x(), t.y(), t.z());
Point3 r1 = R.r1(), r2 = R.r2(), r3 = R.r3(); //columns!
Rot3 cRw(
r1.x(), r2.x(), r3.x(),
-r1.y(), -r2.y(), -r3.y(),
-r1.z(), -r2.z(), -r3.z());
Rot3 wRc = cRw.inverse();
Pose3 actual = Pose3(wRc,t);
EXPECT(assert_equal(expected,actual));
}
int main()
{
int n = 100000; long timeLog, timeLog2; double seconds;
// create a random direction:
double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
Vector v = Vector_(3,x,y,z);
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2), R2 = R.retract(v);
TEST("Rodriguez formula given axis angle", Rot3::rodriguez(v,0.001))
TEST("Rodriguez formula given canonical coordinates", Rot3::rodriguez(v))
TEST("Expmap", R*Rot3::Expmap(v))
TEST("Retract", R.retract(v))
TEST("Logmap", Rot3::Logmap(R.between(R2)))
TEST("localCoordinates", R.localCoordinates(R2))
TEST("Slow rotation matrix",Rot3::Rz(z)*Rot3::Ry(y)*Rot3::Rx(x))
TEST("Fast Rotation matrix", Rot3::RzRyRx(x,y,z))
return 0;
}
SimpleCamera simpleCamera(const Matrix& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
Matrix A = P.topLeftCorner(3, 3);
Vector a = P.col(3);
// do RQ decomposition to get s*K and cRw angles
Matrix sK;
Vector xyz;
boost::tie(sK, xyz) = RQ(A);
// Recover scale factor s and K
double s = sK(2, 2);
Matrix K = sK / s;
// Recover cRw itself, and its inverse
Rot3 cRw = Rot3::RzRyRx(xyz);
Rot3 wRc = cRw.inverse();
// Now, recover T from a = - s K cRw T = - A T
Vector T = -(A.inverse() * a);
return SimpleCamera(Pose3(wRc, T),
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
}
//------------------------------------------------------------------------------
void PreintegrationBase::update(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
deltaTij_ += dt;
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
// Update Jacobians
// TODO(frank): we are repeating some computation here: accessible in F ?
Vector3 j_correctedAcc, j_correctedOmega;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
Matrix3 D_acc_R;
oldRij.rotate(j_correctedAcc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = j_correctedOmega * dt;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * dt;
delVdelBiasOmega_ += D_acc_biasOmega * dt;
}
/* ************************************************************************* */
Vector3 Rot3::CayleyChart::Local(const Rot3& R, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Local Derivative");
// Create a fixed-size matrix
Matrix3 A = R.matrix();
// Mathematica closed form optimization (procrastination?) gone wild:
const double a = A(0, 0), b = A(0, 1), c = A(0, 2);
const double d = A(1, 0), e = A(1, 1), f = A(1, 2);
const double g = A(2, 0), h = A(2, 1), i = A(2, 2);
const double di = d * i, ce = c * e, cd = c * d, fg = f * g;
const double M = 1 + e - f * h + i + e * i;
const double K = -4.0 / (cd * h + M + a * M - g * (c + ce) - b * (d + di - fg));
const double x = a * f - cd + f;
const double y = b * f - ce - c;
const double z = fg - di - d;
return K * Vector3(x, y, z);
}
/* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix3& A) {
double x = -atan2(-A(2, 1), A(2, 2));
Rot3 Qx = Rot3::Rx(-x);
Matrix3 B = A * Qx.matrix();
double y = -atan2(B(2, 0), B(2, 2));
Rot3 Qy = Rot3::Ry(-y);
Matrix3 C = B * Qy.matrix();
double z = -atan2(-C(1, 0), C(1, 1));
Rot3 Qz = Rot3::Rz(-z);
Matrix3 R = C * Qz.matrix();
Vector xyz = Vector3(x, y, z);
return make_pair(R, xyz);
}
/* ************************************************************************* */
Matrix PoseRTV::RRTMnb(const Rot3& att) {
return PoseRTV::RRTMnb(att.rpy());
}
//******************************************************************************
// charts
TEST(Manifold, DefaultChart) {
//DefaultChart<Point2> chart1;
EXPECT(traits<Point2>::Local(Point2(0, 0), Point2(1, 0)) == Vector2(1, 0));
EXPECT(traits<Point2>::Retract(Point2(0, 0), Vector2(1, 0)) == Point2(1, 0));
Vector v2(2);
v2 << 1, 0;
//DefaultChart<Vector2> chart2;
EXPECT(assert_equal(v2, traits<Vector2>::Local(Vector2(0, 0), Vector2(1, 0))));
EXPECT(traits<Vector2>::Retract(Vector2(0, 0), v2) == Vector2(1, 0));
{
typedef Matrix2 ManifoldPoint;
ManifoldPoint m;
//DefaultChart<ManifoldPoint> chart;
m << 1, 3,
2, 4;
// m as per default is in column-major storage mode. So this yields a linear representation of (1, 2, 3, 4)!
EXPECT(assert_equal(Vector(Vector4(1, 2, 3, 4)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(traits<ManifoldPoint>::Retract(m, Vector4(1, 2, 3, 4)) == 2 * m);
}
{
typedef Eigen::Matrix<double, 1, 2> ManifoldPoint;
ManifoldPoint m;
//DefaultChart<ManifoldPoint> chart;
m << 1, 2;
EXPECT(assert_equal(Vector(Vector2(1, 2)), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(traits<ManifoldPoint>::Retract(m, Vector2(1, 2)) == 2 * m);
}
{
typedef Eigen::Matrix<double, 1, 1> ManifoldPoint;
ManifoldPoint m;
//DefaultChart<ManifoldPoint> chart;
m << 1;
EXPECT(assert_equal(Vector(ManifoldPoint::Ones()), Vector(traits<ManifoldPoint>::Local(ManifoldPoint::Zero(), m))));
EXPECT(traits<ManifoldPoint>::Retract(m, ManifoldPoint::Ones()) == 2 * m);
}
//DefaultChart<double> chart3;
Vector v1(1);
v1 << 1;
EXPECT(assert_equal(v1, traits<double>::Local(0, 1)));
EXPECT_DOUBLES_EQUAL(traits<double>::Retract(0, v1), 1, 1e-9);
// Dynamic does not work yet !
Vector z = zero(2), v(2);
v << 1, 0;
//DefaultChart<Vector> chart4;
// EXPECT(assert_equal(traits<Vector>::Local(z, v), v));
// EXPECT(assert_equal(traits<Vector>::Retract(z, v), v));
Vector v3(3);
v3 << 1, 1, 1;
Rot3 I = Rot3::identity();
Rot3 R = I.retract(v3);
//DefaultChart<Rot3> chart5;
EXPECT(assert_equal(v3, traits<Rot3>::Local(I, R)));
EXPECT(assert_equal(traits<Rot3>::Retract(I, v3), R));
// Check zero vector
//DefaultChart<Rot3> chart6;
EXPECT(assert_equal(zero(3), traits<Rot3>::Local(R, R)));
}
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();
}
}
Vec3F toGlobal(Vec3F const & a, Rot3<float> const & r) {
return r.rotateBack(a);
}
void go() {
typedef Vec4<T> Vec;
typedef Vec2<T> Vec2D;
std::cout << std::endl;
std::cout << sizeof(Vec) << std::endl;
std::vector<Vec> vec1; vec1.reserve(50);
std::vector<T> vect(23);
std::vector<Vec> vec2(53);
std::vector<Vec> vec3; vec3.reserve(50234);
Vec x(2.0,4.0,5.0);
Vec y(-3.0,2.0,-5.0);
std::cout << x << std::endl;
std::cout << Vec4<float>(x) << std::endl;
std::cout << Vec4<double>(x) << std::endl;
std::cout << -x << std::endl;
std::cout << x.template get1<2>() << std::endl;
std::cout << y << std::endl;
std::cout << T(3.)*x << std::endl;
std::cout << y*T(0.1) << std::endl;
std::cout << (Vec(1) - y*T(0.1)) << std::endl;
std::cout << mathSSE::sqrt(x) << std::endl;
std::cout << dot(x,y) << std::endl;
std::cout << dotSimple(x,y) << std::endl;
std::cout << "equal" << (x==x ? " " : " not ") << "ok" << std::endl;
std::cout << "not equal" << (x==y ? " not " : " ") << "ok" << std::endl;
Vec z = cross(x,y);
std::cout << z << std::endl;
std::cout << "rotations" << std::endl;
T a = 0.01;
T ca = std::cos(a);
T sa = std::sin(a);
Rot3<T> r1( ca, sa, 0,
-sa, ca, 0,
0, 0, 1);
Rot2<T> r21( ca, sa,
-sa, ca);
Rot3<T> r2(Vec( 0, 1 ,0), Vec( 0, 0, 1), Vec( 1, 0, 0));
Rot2<T> r22(Vec2D( 0, 1), Vec2D( 1, 0));
{
std::cout << "\n3D rot" << std::endl;
Vec xr = r1.rotate(x);
std::cout << x << std::endl;
std::cout << xr << std::endl;
std::cout << r1.rotateBack(xr) << std::endl;
Rot3<T> rt = r1.transpose();
Vec xt = rt.rotate(xr);
std::cout << x << std::endl;
std::cout << xt << std::endl;
std::cout << rt.rotateBack(xt) << std::endl;
std::cout << r1 << std::endl;
std::cout << rt << std::endl;
std::cout << r1*rt << std::endl;
std::cout << r2 << std::endl;
std::cout << r1*r2 << std::endl;
std::cout << r2*r1 << std::endl;
std::cout << r1*r2.transpose() << std::endl;
std::cout << r1.transpose()*r2 << std::endl;
}
{
std::cout << "\n2D rot" << std::endl;
Vec2D xr = r21.rotate(x.xy());
std::cout << x.xy() << std::endl;
std::cout << xr << std::endl;
std::cout << r21.rotateBack(xr) << std::endl;
Rot2<T> rt = r21.transpose();
Vec2D xt = rt.rotate(xr);
std::cout << x.xy() << std::endl;
std::cout << xt << std::endl;
std::cout << rt.rotateBack(xt) << std::endl;
std::cout << r21 << std::endl;
std::cout << rt << std::endl;
std::cout << r21*rt << std::endl;
std::cout << r22 << std::endl;
std::cout << r21*r22 << std::endl;
std::cout << r22*r21 << std::endl;
std::cout << r21*r22.transpose() << std::endl;
std::cout << r21.transpose()*r22 << std::endl;
}
}
void Masseuse::Relax() {
if(!graph->size() || !values->size()){
std::cerr << "Pose graph not loaded. Load poses using 'LoadPosesFromFile'" <<
" before calling Relax()" << std::endl;
throw runtime_error("Initial values or graph empty. Nothing to relax.");
}
// Global problem
ceres::Problem problem;
// Build the problem with the given pose graph
if(options.enable_prior_at_origin && !options.fix_first_pose){
// Add a prior at the origin:
Point3 p = origin.head<3>();
Rot3 rot(origin[3], origin[4], origin[5]);
Pose3 orig(rot, p);
Eigen::Vector6d cov_vec;
cov_vec << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4;
Eigen::MatrixXd m = cov_vec.asDiagonal();
Matrix sqrt_information_matrix = inverseSqrt(m);
ceres::CostFunction* prior_cost_function =
new ceres::AutoDiffCostFunction<PriorPoseCostFunctor<double>,
Sophus::SE3::DoF,
Sophus::SE3::num_parameters>
(new PriorPoseCostFunctor<double>(orig, sqrt_information_matrix));
problem.AddResidualBlock(prior_cost_function, NULL,
values->begin()->second.data());
}
// Now add a binary constraint for all relative and loop closure constraints
for(Factor& f : *graph){
Pose3& T_a = values->at(f.id1);
Pose3& T_b = values->at(f.id2);
Matrix sqrt_information_matrix = inverseSqrt(f.cov);
if(options.enable_switchable_constraints && f.isLCC){
// Use switchable constraints to selectively disable bad LCC's
// during the optimization, see:
// 'Switchable Constraints for Robust Pose Graph SLAM'
ceres::CostFunction* binary_cost_function =
new ceres::AutoDiffCostFunction<SwitchableBinaryPoseCostFunctor
<double>, Sophus::SE3::DoF,
Sophus::SE3::num_parameters,
Sophus::SE3::num_parameters,
1>
(new SwitchableBinaryPoseCostFunctor<double>(f.rel_pose,
sqrt_information_matrix));
HuberLoss* loss = new HuberLoss(options.huber_loss_delta);
problem.AddResidualBlock(binary_cost_function, loss,
T_a.data(),
T_b.data(),
&f.switch_variable);
// Constrain the switch variable to be between 0 and 1
problem.SetParameterLowerBound(&f.switch_variable, 0, 0.0);
problem.SetParameterUpperBound(&f.switch_variable, 0, 1.0);
// Add a prior to anchor the switch variable at its initial value
ceres::CostFunction* prior_cost_function =
new ceres::AutoDiffCostFunction<PriorCostFunctor<double>,
1, 1>(new PriorCostFunctor<double>(f.switch_variable,
options.switch_variable_prior_cov,
0));
problem.AddResidualBlock(prior_cost_function, NULL,
&f.switch_variable);
}else{
Matrix sqrt_information_matrix = inverseSqrt(f.cov);
ceres::CostFunction* binary_cost_function =
new ceres::AutoDiffCostFunction<BinaryPoseCostFunctor
<double>, Sophus::SE3::DoF,
Sophus::SE3::num_parameters,
Sophus::SE3::num_parameters>
(new BinaryPoseCostFunctor<double>(f.rel_pose, sqrt_information_matrix));
HuberLoss* loss = new HuberLoss(options.huber_loss_delta);
problem.AddResidualBlock(binary_cost_function, loss,
T_a.data(),
T_b.data());
}
if(options.enable_z_prior){
// Add a prior on z so that it anchors the height to the initial value
// this assumes roughly planar motion to avoid the z drift.
double initial_z = origin[2]; // first three elements are x, y, z
// The last parameter is the index of the z in the SO3 data structure
// It is [i j k w x y z]
ceres::CostFunction* prior_cost_function =
new ceres::AutoDiffCostFunction<PriorCostFunctor<double>,
1, 7>(new PriorCostFunctor<double>(initial_z,
options.cov_z_prior,
6));
problem.AddResidualBlock(prior_cost_function, NULL,
T_b.data());
}
}
if(options.fix_first_pose && problem.HasParameterBlock(values->begin()->second.data())){
problem.SetParameterBlockConstant(values->begin()->second.data());
}
ceres::LocalParameterization* local_param =
new ceres::AutoDiffLocalParameterization
<Sophus::masseuse::AutoDiffLocalParamSE3, 7, 6>;
for(auto &pair : *values){
if(problem.HasParameterBlock(pair.second.data())){
problem.SetParameterization(pair.second.data(),
local_param);
}
}
ceres::Solver::Options ceres_options;
ceres_options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
ceres_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
ceres_options.minimizer_progress_to_stdout = options.print_minimizer_progress;
ceres_options.max_num_iterations = options.num_iterations;
ceres_options.update_state_every_iteration =
options.update_state_every_iteration;
ceres_options.check_gradients = options.check_gradients;
ceres_options.function_tolerance = options.function_tolearnce;
ceres_options.parameter_tolerance = options.parameter_tolerance;
ceres_options.gradient_tolerance = options.gradient_tolerance;
if(options.print_error_statistics){
std::cerr << "BEFORE RELAXATION:" << std::endl;
PrintErrorStatistics(*values);
std::cerr << std::endl;
// if(options.enable_switchable_constraints){
// std::cerr << "switch variables BEFORE optimizing: " << std::endl;
// for(const Factor& f : *graph){
// if(f.isLCC){
// fprintf(stderr, "%1.3f ",
// f.switch_variable);
// }
// }
// std::cerr << std::endl;
// }
// double initial_cost = 0.0;
// std::vector<double> residuals(problem.NumResiduals());
// problem.Evaluate(Problem::EvaluateOptions(), &initial_cost, &residuals
// , NULL, NULL);
// std::cerr << "num residual blocks: " << problem.NumResidualBlocks() <<
// std::endl;
// std::cerr << "Cost BEFORE optimizing: " << initial_cost << std::endl;
}
ceres::Solver::Summary summary;
std::cerr << "Relaxing graph...." << std::endl;
double ceres_time = masseuse::Tic();
ceres::Solve(ceres_options, &problem, &summary);
ceres_time = masseuse::Toc(ceres_time);
fprintf(stderr, "Optimization finished in %fs \n",
ceres_time);
if(options.print_full_report){
std::cerr << summary.FullReport() << std::endl;
}
if(options.print_brief_report){
std::cerr << summary.BriefReport() << std::endl;
}
if(options.print_error_statistics){
std::cerr << std::endl << "AFTER REALXATION:" << std::endl;
PrintErrorStatistics(*values);
// if(options.enable_switchable_constraints){
// std::cerr << "switch variables AFTER optimizing: " << std::endl;
// for(const Factor& f : *graph){
// if(f.isLCC){
// fprintf(stderr, "%1.3f ",
// f.switch_variable);
// }
// }
// std::cerr << std::endl;
// }
// double final_cost = 0.0;
// std::vector<double> residuals(problem.NumResiduals());
// problem.Evaluate(Problem::EvaluateOptions(), &final_cost, &residuals,
// NULL, NULL);
//std::cerr << "Cost AFTER optimizing: " << final_cost << std::endl;
// Eigen::Map<Eigen::VectorXd> vec_residuals(residuals.data(), residuals.size());
// std::cerr << "Residuals: " << vec_residuals << std::endl;
problem.NumResiduals();
}
// Save optimization result in a binary file
if(options.save_results_binary){
output_abs_poses.clear();
Eigen::Vector6d absPoseVec;
ceres::Covariance::Options cov_options;
ceres::Covariance covariance(cov_options);
vector<pair<const double*, const double*> > covariance_blocks;
for(const auto& kvp : *values){
const Pose3& pose = kvp.second;
Point3 p = pose.translation();
Rot3 R = pose.so3();
absPoseVec.head<3>() = p;
// unpack Euler angles: roll, pitch, yaw
absPoseVec.tail<3>() = R.matrix().eulerAngles(0, 1, 2);
// Now get the covariance for this pose
covariance_blocks.clear();
covariance_blocks.push_back(std::make_pair(pose.data(), pose.data()));
CHECK(covariance.Compute(covariance_blocks, &problem));
double cov[6*6];
covariance.GetCovarianceBlockInTangentSpace(
pose.data(),
pose.data(),
cov);
// convert to an Eigen matrix
Eigen::Map < Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > cov_mat(
cov);
AbsPose absPose(kvp.first, absPoseVec, cov_mat);
output_abs_poses.push_back(absPose);
}
SaveAbsPG(options.binary_output_path);
}
}
/* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
Vec3F toLocal(Vec3F const & a, Rot3<float> const & r) {
return r.rotate(a);
}
namespace example1 {
const string filename = findExampleDataFile("5pointExample1.txt");
SfM_data data;
bool readOK = readBAL(filename, data);
Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation();
PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), Cal3_S2());
EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
double baseline = 0.1; // actual baseline of the camera
Point2 pA(size_t i) {
return data.tracks[i].measurements[0].second;
}
Point2 pB(size_t i) {
return data.tracks[i].measurements[1].second;
}
Vector vA(size_t i) {
return EssentialMatrix::Homogeneous(pA(i));
}
Vector vB(size_t i) {
return EssentialMatrix::Homogeneous(pB(i));
}
//*************************************************************************
TEST (EssentialMatrixFactor, testData) {
CHECK(readOK);
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z())
* c1Rc2.matrix();
EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
// Check some projections
EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8));
EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8));
EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8));
EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));
// Check homogeneous version
EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8));
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
}
//*************************************************************************
TEST (EssentialMatrixFactor, factor) {
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor factor(1, pA(i), pB(i), model1);
// Check evaluation
Vector expected(1);
expected << 0;
Matrix Hactual;
Vector actual = factor.evaluateError(trueE, Hactual);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected;
typedef Eigen::Matrix<double,1,1> Vector1;
Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected, Hactual, 1e-8));
}
}
//*************************************************************************
TEST (EssentialMatrixFactor, minimization) {
// Here we want to optimize directly on essential matrix constraints
// Yi Ma's algorithm (Ma01ijcv) is a bit cumbersome to implement,
// but GTSAM does the equivalent anyway, provided we give the right
// factors. In this case, the factors are the constraints.
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor(1, pA(i), pB(i), model1));
// Check error at ground truth
Values truth;
truth.insert(1, trueE);
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Check error at initial estimate
Values initial;
EssentialMatrix initialE = trueE.retract(
(Vector(5) << 0.1, -0.1, 0.1, 0.1, -0.1).finished());
initial.insert(1, initialE);
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT_DOUBLES_EQUAL(643.26, graph.error(initial), 1e-2);
#else
EXPECT_DOUBLES_EQUAL(639.84, graph.error(initial), 1e-2);
#endif
// Optimize
LevenbergMarquardtParams parameters;
LevenbergMarquardtOptimizer optimizer(graph, initial, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(1);
EXPECT(assert_equal(trueE, actual, 1e-1));
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
// Check errors individually
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, actual.error(vA(i), vB(i)), 1e-6);
}
//*************************************************************************
TEST (EssentialMatrixFactor2, factor) {
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor2 factor(100, i, pA(i), pB(i), model2);
// Check evaluation
Point3 P1 = data.tracks[i].p, P2 = data.cameras[1].pose().transform_to(P1);
const Point2 pi = PinholeBase::Project(P2);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
}
}
//*************************************************************************
TEST (EssentialMatrixFactor2, minimization) {
// Here we want to optimize for E and inverse depths at the same time
// We start with a factor graph and add constraints to it
// Noise sigma is 1cm, assuming metric measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));
// Check error at ground truth
Values truth;
truth.insert(100, trueE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
//*************************************************************************
// Below we want to optimize for an essential matrix specified in a
// body coordinate frame B which is rotated with respect to the camera
// frame C, via the rotation bRc.
// The "true E" in the body frame is then
EssentialMatrix bodyE = cRb.inverse() * trueE;
//*************************************************************************
TEST (EssentialMatrixFactor3, factor) {
for (size_t i = 0; i < 5; i++) {
EssentialMatrixFactor3 factor(100, i, pA(i), pB(i), cRb, model2);
// Check evaluation
Point3 P1 = data.tracks[i].p;
const Point2 pi = camera2.project(P1);
Point2 reprojectionError(pi - pB(i));
Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2;
double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
EXPECT(assert_equal(Hexpected2, Hactual2, 1e-8));
}
}
//*************************************************************************
TEST (EssentialMatrixFactor3, minimization) {
// As before, we start with a factor graph and add constraints to it
NonlinearFactorGraph graph;
for (size_t i = 0; i < 5; i++)
// but now we specify the rotation bRc
graph.add(EssentialMatrixFactor3(100, i, pA(i), pB(i), cRb, model2));
// Check error at ground truth
Values truth;
truth.insert(100, bodyE);
for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
// Optimize
LevenbergMarquardtParams parameters;
// parameters.setVerbosity("ERROR");
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
Values result = optimizer.optimize();
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}
} // namespace example1
