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