Skip to content

Commit

Permalink
WIP: save work.
Browse files Browse the repository at this point in the history
  • Loading branch information
oddkiva committed Apr 30, 2024
1 parent b5bfb7e commit cb4118b
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 20 deletions.
19 changes: 10 additions & 9 deletions cpp/src/DO/Sara/MultiViewGeometry/MinimalSolvers/P3PSolver.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,12 +11,11 @@

#pragma once

#include <DO/Sara/Core/EigenFormatInterop.hpp>
#include <DO/Sara/Core/Math/UsualFunctions.hpp>
#include <DO/Sara/MultiViewGeometry/PnP/LambdaTwist.hpp>
#include <DO/Sara/MultiViewGeometry/PointRayCorrespondenceList.hpp>

#include <fmt/format.h>


namespace DO::Sara {

Expand All @@ -30,9 +29,9 @@ namespace DO::Sara {
using data_point_type = std::array<TensorView_<T, 2>, 2>;
using model_type = Eigen::Matrix<T, 3, 4>;

inline auto
operator()(const tensor_view_type& scene_points,
const tensor_view_type& rays) const -> std::vector<model_type>
inline auto operator()(const tensor_view_type& scene_points,
const tensor_view_type& rays) const
-> std::vector<model_type>
{
const auto sp_mat_ = scene_points.colmajor_view().matrix();

Expand Down Expand Up @@ -108,6 +107,7 @@ namespace DO::Sara {
"The dimension of scene points is incorrect. "
"They must either 3D (Euclidean) or 4D (homogeneous)!"};

#if defined(USE_CALIBRATION_MATRIX)
// Project the camera coordinates to the image plane.
//
// The result is a list of pixel coordinates.
Expand All @@ -125,6 +125,10 @@ namespace DO::Sara {
auto u2 = Eigen::MatrixXd{2, rays.cols()};
for (auto i = 0; i < u2.cols(); ++i)
u2.col(i) = C->project(rays.col(i));
#else
const Eigen::MatrixXd u1 = X_camera.colwise().hnormalized();
const Eigen::MatrixXd u2 = rays.colwise().hnormalized();
#endif

// Check the cheirality w.r.t. the candidate pose.
const auto cheiral = X_camera.row(2).array() > 0;
Expand All @@ -134,7 +138,7 @@ namespace DO::Sara {
const auto ε_squared = (u2 - u1).colwise().squaredNorm().array();
const auto ε_small = ε_squared < ε_max;

#if 0
#if defined(CHECK_PNP_RESIDUALS)
fmt::print("Pose:\n{}\n", T);
const auto ε_debug = Eigen::VectorXd{ε_squared.sqrt()};
const auto col_max = std::min(Eigen::Index{10}, u2.cols());
Expand Down Expand Up @@ -162,7 +166,4 @@ namespace DO::Sara {
}
};


//! @}

} // namespace DO::Sara
16 changes: 8 additions & 8 deletions cpp/src/DO/Sara/SfM/BuildingBlocks/CameraPoseEstimator.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,11 @@ namespace DO::Sara {

//! @brief Set robust estimation parameters.
auto
set_estimation_params(const PixelUnit error_max = 5._px,
const int ransac_iter_max = 10000,
set_estimation_params([[maybe_unused]] const PixelUnit error_max = 5._px,
const int ransac_iter_max = 1000,
const double ransac_confidence_min = 0.99) -> void
{
_inlier_predicate.ε = error_max.value;
_inlier_predicate.ε = 0.1; // error_max.value;
_ransac_iter_max = ransac_iter_max;
_ransac_confidence_min = ransac_confidence_min;
}
Expand All @@ -54,11 +54,11 @@ namespace DO::Sara {
const CameraIntrinsicModel&)
-> std::tuple<PoseMatrix, Inlier, MinimalSample>;

auto
estimate_pose(const std::vector<FeatureTrack>&,
const CameraPoseGraph::Vertex, //
const CameraIntrinsicModel&, //
const PointCloudGenerator&) -> std::pair<PoseMatrix, bool>;
auto estimate_pose(const std::vector<FeatureTrack>&,
const CameraPoseGraph::Vertex, //
const CameraIntrinsicModel&, //
const PointCloudGenerator&)
-> std::pair<PoseMatrix, bool>;

private:
P3PSolver<double> _solver;
Expand Down
15 changes: 12 additions & 3 deletions cpp/src/DO/Sara/SfM/Odometry/OdometryPipeline.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -315,9 +315,18 @@ auto OdometryPipeline::grow_geometry() -> bool
const auto q_global = Eigen::Quaterniond{_current_global_rotation};
auto angles = calculate_yaw_pitch_roll(q_global);
static constexpr auto degrees = 180. / M_PI;
SARA_LOGI(logger, "Global yaw = {:0.3f} deg", angles(0) * degrees);
SARA_LOGI(logger, "Global pitch = {:0.3f} deg", angles(1) * degrees);
SARA_LOGI(logger, "Global roll = {:0.3f} deg", angles(2) * degrees);
SARA_LOGI(logger, "[Rel] Global yaw = {:0.3f} deg", angles(0) * degrees);
SARA_LOGI(logger, "[Rel] Global pitch = {:0.3f} deg", angles(1) * degrees);
SARA_LOGI(logger, "[Rel] Global roll = {:0.3f} deg", angles(2) * degrees);


const auto R_abs = _pose_graph[_pose_curr].pose.q.toRotationMatrix();
const auto q_global_2 =
Eigen::Quaterniond{P * R_abs.transpose() * P.transpose()};
angles = calculate_yaw_pitch_roll(q_global_2);
SARA_LOGI(logger, "[Rel] Global yaw = {:0.3f} deg", angles(0) * degrees);
SARA_LOGI(logger, "[Rel] Global pitch = {:0.3f} deg", angles(1) * degrees);
SARA_LOGI(logger, "[Rel] Global roll = {:0.3f} deg", angles(2) * degrees);

return true;
}

0 comments on commit cb4118b

Please sign in to comment.