Skip to content

Commit

Permalink
more compile updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@rsoussan
rsoussan committed Jul 22, 2024
1 parent bce16c8 commit 93ab545
Show file tree
Hide file tree
Showing 6 changed files with 77 additions and 66 deletions.
3 changes: 2 additions & 1 deletion localization/sparse_mapping/include/sparse_mapping/reprojection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@

namespace camera {
class CameraModel;
class CameraParameters;
}

namespace cv {
Expand Down Expand Up @@ -83,7 +84,7 @@ void BundleAdjust(std::vector<std::map<int, int> > const& pid_to_cid_fid,
*
**/
void BundleAdjustSmallSet(std::vector<Eigen::Matrix2Xd> const& features_n,
double focal_length,
std::vector<double> focal_length_n,
std::vector<Eigen::Affine3d> * cam_t_global_n,
Eigen::Matrix3Xd * pid_to_xyz,
ceres::LossFunction * loss,
Expand Down
9 changes: 5 additions & 4 deletions localization/sparse_mapping/include/sparse_mapping/sparse_map.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,13 +178,12 @@ struct SparseMap {
* Return the parameters of the camera used to construct the map.
**/
const camera::CameraParameters& camera_params(int cid) const {
return camera_id_to_camera_params_[cid_to_camera_id[cid]];
return camera_id_to_camera_params_[cid_to_camera_id_[cid]];
}
camera::CameraParameters GetCameraParameters(int cid) const {
return camera_id_to_camera_params_[cid_to_camera_id[cid]];
return camera_id_to_camera_params_[cid_to_camera_id_[cid]];
}
// void SetCameraParameters(int cid, camera::CameraParameters& camera_params) {cid_to_camera_params_[cid] =
// camera_params;}

/**
* Return the number of observations. Use this number to divide the final error to find the average pixel error.
**/
Expand Down Expand Up @@ -218,10 +217,12 @@ struct SparseMap {

// detect features with opencv
void DetectFeaturesFromFile(std::string const& filename,
const camera::CameraParameters& camera_params,
bool multithreaded,
cv::Mat* descriptors,
Eigen::Matrix2Xd* keypoints);
void DetectFeatures(cv::Mat const& image,
const camera::CameraParameters& camera_params,
bool multithreaded,
cv::Mat* descriptors,
Eigen::Matrix2Xd* keypoints);
Expand Down
6 changes: 4 additions & 2 deletions localization/sparse_mapping/include/sparse_mapping/tensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,14 +146,16 @@ namespace sparse_mapping {

// Filter the matches by a geometric constraint. Compute the essential matrix.
void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2,
std::vector<cv::DMatch> const& matches, camera::CameraParameters const& camera_params,
std::vector<cv::DMatch> const& matches, camera::CameraParameters const& camera_params1,
camera::CameraParameters const& camera_params2,
std::vector<cv::DMatch>* inlier_matches, std::vector<size_t>* vec_inliers,
Eigen::Matrix3d* essential_matrix, const int ransac_iterations = 4096);

void BuildMapFindEssentialAndInliers(const Eigen::Matrix2Xd & keypoints1,
const Eigen::Matrix2Xd & keypoints2,
const std::vector<cv::DMatch> & matches,
camera::CameraParameters const& camera_params,
camera::CameraParameters const& camera_params1,
camera::CameraParameters const& camera_params2,
bool compute_inliers_only,
size_t cam_a_idx, size_t cam_b_idx,
std::mutex * match_mutex,
Expand Down
10 changes: 6 additions & 4 deletions localization/sparse_mapping/src/reprojection.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@ void BundleAdjust(std::vector<std::map<int, int> > const& pid_to_cid_fid,
&cid_to_cam_t_global->at(cid_fid.first).translation()[0],
&camera_aa_storage[3 * cid_fid.first],
&p_pid_to_xyz->at(pid)[0],
&focal_length[cid_fid.first]);
&focal_lengths[cid_fid.first]);

if (fix_all_cameras || (cid_fid.first < first || cid_fid.first > last) ||
fixed_cameras.find(cid_fid.first) != fixed_cameras.end()) {
Expand All @@ -182,7 +182,7 @@ void BundleAdjust(std::vector<std::map<int, int> > const& pid_to_cid_fid,
problem.SetParameterBlockConstant(&p_pid_to_xyz->at(pid)[0]);
}
}
for (const auto& focal_length : focal_lengths) {
for (auto& focal_length : focal_lengths) {
problem.SetParameterBlockConstant(&focal_length);
}
}
Expand Down Expand Up @@ -236,10 +236,12 @@ void BundleAdjustSmallSet(std::vector<Eigen::Matrix2Xd> const& features_n,
&cam_t_global_n->at(cid).translation()[0],
&aa.at(cid)[0],
&pid_to_xyz->col(pid)[0],
&focal_length[cid]);
&focal_length_n[cid]);
}
}
problem.SetParameterBlockConstant(&focal_length[cid]);
for (size_t cid = 0; cid < n_cameras; ++cid) {
problem.SetParameterBlockConstant(&focal_length_n[cid]);
}

// Solve the problem
ceres::Solve(options, &problem, summary);
Expand Down
101 changes: 53 additions & 48 deletions localization/sparse_mapping/src/sparse_map.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,9 +132,8 @@ SparseMap::SparseMap(const std::vector<Eigen::Affine3d>& cid_to_cam_t, const std

// Form a sparse map by reading a text file from disk. This is for comparing
// bundler, nvm or theia maps.
SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vector<std::string> const& all_image_files)
: camera_params_(Eigen::Vector2i(640, 480), Eigen::Vector2d::Constant(300),
Eigen::Vector2d(320, 240)) /* these are placeholders and must be changed */ {
SparseMap::SparseMap(bool bundler_format, std::string const& filename,
std::vector<std::string> const& all_image_files) {
SetDefaultLocParams();
std::string ext = ff_common::file_extension(filename);
boost::to_lower(ext);
Expand Down Expand Up @@ -325,50 +324,52 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) {
if (!ReadProtobufFrom(input, &camera)) {
LOG(FATAL) << "Failed to parse camera.";
}
camera::CameraParams camera_params;
assert(camera.focal_length_size() == 2);
assert(camera.optical_offset_size() == 2);
assert(camera.distorted_image_size_size() == 2);
assert(camera.undistorted_image_size_size() == 2);
typedef Eigen::Vector2d V2d;
typedef Eigen::Vector2i V2i;
camera_params.SetFocalLength(V2d(camera.focal_length(0),
camera.focal_length(1)));
camera_params.SetOpticalOffset(V2d(camera.optical_offset(0),
camera.optical_offset(1)));
camera_params.SetDistortedSize(V2i(camera.distorted_image_size(0),
camera.distorted_image_size(1)));
camera_params.SetUndistortedSize(V2i(camera.undistorted_image_size(0),
camera.undistorted_image_size(1)));
const V2d focal_length(camera.focal_length(0),
camera.focal_length(1));
const V2d optical_center(camera.optical_offset(0),
camera.optical_offset(1));
const V2i distorted_image_size(camera.distorted_image_size(0),
camera.distorted_image_size(1));
Eigen::VectorXd distortion(camera.distortion_size());
for (int i = 0; i < camera.distortion_size(); i++) {
distortion[i] = camera.distortion(i);
}
camera_params.SetDistortion(distortion);
camera::CameraParameters camera_params(distorted_image_size, focal_length, optical_center, distortion);
camera_params.SetDistortedSize(V2i(camera.distorted_image_size(0),
camera.distorted_image_size(1)));
camera_params.SetUndistortedSize(V2i(camera.undistorted_image_size(0),
camera.undistorted_image_size(1)));
camera_id_to_camera_params_.emplace_back(camera_params);
}
} else {
camera::CameraParams camera_params;
assert(map.camera().focal_length_size() == 2);
assert(map.camera().optical_offset_size() == 2);
assert(map.camera().distorted_image_size_size() == 2);
assert(map.camera().undistorted_image_size_size() == 2);
typedef Eigen::Vector2d V2d;
typedef Eigen::Vector2i V2i;
camera_params.SetFocalLength(V2d(map.camera().focal_length(0),
map.camera().focal_length(1)));
camera_params.SetOpticalOffset(V2d(map.camera().optical_offset(0),
map.camera().optical_offset(1)));
camera_params.SetDistortedSize(V2i(map.camera().distorted_image_size(0),
map.camera().distorted_image_size(1)));
camera_params.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0),
map.camera().undistorted_image_size(1)));
Eigen::VectorXd distortion(map.camera().distortion_size());
for (int i = 0; i < map.camera().distortion_size(); i++) {
distortion[i] = map.camera().distortion(i);
}
camera_params.SetDistortion(distortion);
camera_id_to_camera_params_.emplace_back(camera_params);
assert(map.camera().focal_length_size() == 2);
assert(map.camera().optical_offset_size() == 2);
assert(map.camera().distorted_image_size_size() == 2);
assert(map.camera().undistorted_image_size_size() == 2);
typedef Eigen::Vector2d V2d;
typedef Eigen::Vector2i V2i;
const V2d focal_length(map.camera().focal_length(0),
map.camera().focal_length(1));
const V2d optical_center(map.camera().optical_offset(0),
map.camera().optical_offset(1));
const V2i distorted_image_size(map.camera().distorted_image_size(0),
map.camera().distorted_image_size(1));
Eigen::VectorXd distortion(map.camera().distortion_size());
for (int i = 0; i < map.camera().distortion_size(); i++) {
distortion[i] = map.camera().distortion(i);
}
camera::CameraParameters camera_params(distorted_image_size, focal_length, optical_center, distortion);
camera_params.SetDistortedSize(V2i(map.camera().distorted_image_size(0),
map.camera().distorted_image_size(1)));
camera_params.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0),
map.camera().undistorted_image_size(1)));
camera_id_to_camera_params_.emplace_back(camera_params);
}

int num_frames = map.num_frames();
Expand Down Expand Up @@ -568,20 +569,6 @@ void SparseMap::Save(const std::string & protobuf_file) const {
map.set_descriptor_depth(0);

map.set_num_cameras(camera_id_to_camera_params_.size());
for (const auto& camera_params : camera_id_to_camera_params_) {
sparse_mapping_protobuf::CameraModel* camera = map.add_camera();
camera->add_focal_length(camera_params.GetFocalVector()[0]);
camera->add_focal_length(camera_params.GetFocalVector()[1]);
camera->add_optical_offset(camera_params.GetOpticalOffset()[0]);
camera->add_optical_offset(camera_params.GetOpticalOffset()[1]);
camera->add_distorted_image_size(camera_params.GetDistortedSize()[0]);
camera->add_distorted_image_size(camera_params.GetDistortedSize()[1]);
camera->add_undistorted_image_size(camera_params.GetUndistortedSize()[0]);
camera->add_undistorted_image_size(camera_params.GetUndistortedSize()[1]);
for (int i = 0; i < camera_params.GetDistortion().size(); i++) {
camera->add_distortion(camera_params.GetDistortion()[i]);
}
}

CHECK(cid_to_filename_.size() == cid_to_keypoint_map_.size())
<< "Number of CIDs in filenames and keypoint map do not match";
Expand All @@ -606,6 +593,24 @@ void SparseMap::Save(const std::string & protobuf_file) const {
LOG(FATAL) << "Failed to write map to file.";
}

for (const auto& camera_params : camera_id_to_camera_params_) {
sparse_mapping_protobuf::CameraModel camera;
camera.add_focal_length(camera_params.GetFocalVector()[0]);
camera.add_focal_length(camera_params.GetFocalVector()[1]);
camera.add_optical_offset(camera_params.GetOpticalOffset()[0]);
camera.add_optical_offset(camera_params.GetOpticalOffset()[1]);
camera.add_distorted_image_size(camera_params.GetDistortedSize()[0]);
camera.add_distorted_image_size(camera_params.GetDistortedSize()[1]);
camera.add_undistorted_image_size(camera_params.GetUndistortedSize()[0]);
camera.add_undistorted_image_size(camera_params.GetUndistortedSize()[1]);
for (int i = 0; i < camera_params.GetDistortion().size(); i++) {
camera.add_distortion(camera_params.GetDistortion()[i]);
}
if (!WriteProtobufTo(camera, output)) {
LOG(FATAL) << "Failed to write camera to file.";
}
}

// write the frames
for (size_t cid = 0; cid < cid_to_filename_.size(); cid++) {
sparse_mapping_protobuf::Frame frame;
Expand Down
14 changes: 7 additions & 7 deletions localization/sparse_mapping/src/tensor.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1183,8 +1183,6 @@ void MergeMaps(sparse_mapping::SparseMap * A_in,
sparse_mapping::SparseMap & C = *C_out;

// Basic sanity checks (not exhaustive)
if ( !(A.GetCameraParameters() == B.GetCameraParameters()) )
LOG(FATAL) << "The input maps don't have the same camera parameters.";
if ( !(A.detector_ == B.detector_) )
LOG(FATAL) << "The input maps don't have the same detector and/or descriptor.";

Expand Down Expand Up @@ -1222,8 +1220,8 @@ void MergeMaps(sparse_mapping::SparseMap * A_in,
}
// Append B's camera params to A's.
// TODO(rsoussan): Check for equality of camera params and consolidate
for (const auto& camera_params : B.camera_ids_to_camera_params_) {
A.camera_ids_to_camera_params_.append(camera_params);
for (const auto& camera_params : B.camera_id_to_camera_params_) {
A.camera_id_to_camera_params_.emplace_back(camera_params);
}

// Create cid_fid_to_pid_ for both maps, to be able to go from cid_fid to pid.
Expand Down Expand Up @@ -1965,8 +1963,10 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X
observationsb.col(i) = keypoints2.col(matches[i].trainIdx);
}

std::pair<size_t, size_t> image_size(camera_params.GetUndistortedSize()[0],
camera_params.GetUndistortedSize()[1]);
std::pair<size_t, size_t> image_size1(camera_params1.GetUndistortedSize()[0],
camera_params1.GetUndistortedSize()[1]);
std::pair<size_t, size_t> image_size2(camera_params2.GetUndistortedSize()[0],
camera_params2.GetUndistortedSize()[1]);
Eigen::Matrix3d k1 = camera_params1.GetIntrinsicMatrix<camera::UNDISTORTED_C>();
Eigen::Matrix3d k2 = camera_params2.GetIntrinsicMatrix<camera::UNDISTORTED_C>();

Expand All @@ -1976,7 +1976,7 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X

if (!interest_point::RobustEssential(k1, k2, observationsa, observationsb,
essential_matrix, vec_inliers,
image_size, image_size,
image_size1, image_size2,
&error_max,
max_expected_error, ransac_iterations)) {
VLOG(2) << " | Estimation of essential matrix failed!\n";
Expand Down

0 comments on commit 93ab545

Please sign in to comment.