//*************************************************************************
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);
}
/* ************************************************************************* */
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);
}
//------------------------------------------------------------------------------
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;
}
/* ************************************************************************* */
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);
}
//------------------------------------------------------------------------------
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;
}
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);
}
