diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch
index 95d567e..ee07bc6 100644
--- a/launch/odometry_eval.launch
+++ b/launch/odometry_eval.launch
@@ -11,8 +11,10 @@
-
+
+
+
camera_model: '$(arg camera_model)'
@@ -24,10 +26,12 @@
tracker_delta_pixel_error_threshold: 3.0
tracker_error_threshold: 20.0
min_features_per_region: 10
- pnp_inlier_threshold: 5.0
- pnp_iterations: 1000
+ pnp_inlier_threshold: 3.0
+ pnp_iterations: 2000
bundle_adjustment_max_iterations: 1000
+ bundle_adjustment_loss_coefficient: 0.05
bundle_adjustment_logging: true
+ bundle_adjustment_num_threads: 20
diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch
index 1ede7fb..ef58dca 100644
--- a/launch/reconstruction_eval.launch
+++ b/launch/reconstruction_eval.launch
@@ -23,10 +23,12 @@
tracker_delta_pixel_error_threshold: 3.0
tracker_error_threshold: 20.0
min_features_per_region: 10
- max_reprojection_error: 0.0
+ max_reprojection_error: 5.0
min_triangulation_angle: 3.0
bundle_adjustment_max_iterations: 1000
+ bundle_adjustment_loss_coefficient: 0.1
bundle_adjustment_logging: true
+ bundle_adjustment_num_threads: 20
diff --git a/src/data/frame.cc b/src/data/frame.cc
index 648d864..663ef4b 100644
--- a/src/data/frame.cc
+++ b/src/data/frame.cc
@@ -6,8 +6,10 @@ namespace omni_slam
namespace data
{
-Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model)
- : id_(id),
+int Frame::lastFrameId_ = 0;
+
+Frame::Frame(cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model)
+ : id_(lastFrameId_++),
image_(image.clone()),
depthImage_(depth_image.clone()),
pose_(pose),
@@ -19,8 +21,8 @@ Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &camera_model)
- : id_(id),
+Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
+ : id_(lastFrameId_++),
image_(image.clone()),
depthImage_(depth_image.clone()),
timeSec_(time),
@@ -30,8 +32,8 @@ Frame::Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, ca
hasDepth_ = true;
}
-Frame::Frame(const int id, cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model)
- : id_(id),
+Frame::Frame(cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model)
+ : id_(lastFrameId_++),
image_(image.clone()),
pose_(pose),
invPose_(util::TFUtil::InversePoseMatrix(pose)),
@@ -42,8 +44,8 @@ Frame::Frame(const int id, cv::Mat &image, Matrix &pose, double t
hasDepth_ = false;
}
-Frame::Frame(const int id, cv::Mat &image, double time, camera::CameraModel<> &camera_model)
- : id_(id),
+Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
+ : id_(lastFrameId_++),
timeSec_(time),
cameraModel_(camera_model)
{
@@ -136,6 +138,14 @@ void Frame::SetDepthImage(cv::Mat &depth_image)
hasDepth_ = true;
}
+void Frame::SetEstimatedPose(const Matrix &pose, const std::vector &landmark_ids)
+{
+ poseEstimate_ = pose;
+ invPoseEstimate_ = util::TFUtil::InversePoseMatrix(pose);
+ estLandmarkIds_ = std::unordered_set(landmark_ids.begin(), landmark_ids.end());
+ hasPoseEstimate_ = true;
+}
+
void Frame::SetEstimatedPose(const Matrix &pose)
{
poseEstimate_ = pose;
@@ -143,6 +153,14 @@ void Frame::SetEstimatedPose(const Matrix &pose)
hasPoseEstimate_ = true;
}
+void Frame::SetEstimatedInversePose(const Matrix &pose, const std::vector &landmark_ids)
+{
+ invPoseEstimate_ = pose;
+ poseEstimate_ = util::TFUtil::InversePoseMatrix(pose);
+ estLandmarkIds_ = std::unordered_set(landmark_ids.begin(), landmark_ids.end());
+ hasPoseEstimate_ = true;
+}
+
void Frame::SetEstimatedInversePose(const Matrix &pose)
{
invPoseEstimate_ = pose;
@@ -165,6 +183,11 @@ bool Frame::HasEstimatedPose() const
return hasPoseEstimate_;
}
+bool Frame::IsEstimatedByLandmark(const int landmark_id) const
+{
+ return estLandmarkIds_.find(landmark_id) != estLandmarkIds_.end();
+}
+
void Frame::CompressImages()
{
if (isCompressed_)
@@ -203,5 +226,10 @@ bool Frame::IsCompressed() const
return isCompressed_;
}
+const double Frame::GetTime() const
+{
+ return timeSec_;
+}
+
}
}
diff --git a/src/data/frame.h b/src/data/frame.h
index 40d1c93..7ec88eb 100644
--- a/src/data/frame.h
+++ b/src/data/frame.h
@@ -3,6 +3,7 @@
#include
#include
+#include
#include "camera/camera_model.h"
using namespace Eigen;
@@ -15,10 +16,10 @@ namespace data
class Frame
{
public:
- Frame(const int id, cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model);
- Frame(const int id, cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
- Frame(const int id, cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model);
- Frame(const int id, cv::Mat &image, double time, camera::CameraModel<> &camera_model);
+ Frame(cv::Mat &image, cv::Mat &depth_image, Matrix &pose, double time, camera::CameraModel<> &camera_model);
+ Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
+ Frame(cv::Mat &image, Matrix &pose, double time, camera::CameraModel<> &camera_model);
+ Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model);
Frame(const Frame &other);
const Matrix& GetPose() const;
@@ -29,14 +30,18 @@ class Frame
const Matrix& GetEstimatedPose() const;
const Matrix& GetEstimatedInversePose() const;
const int GetID() const;
+ const double GetTime() const;
bool HasPose() const;
bool HasDepthImage() const;
bool HasEstimatedPose() const;
+ bool IsEstimatedByLandmark(const int landmark_id) const;
void SetPose(Matrix &pose);
void SetDepthImage(cv::Mat &depth_image);
+ void SetEstimatedPose(const Matrix &pose, const std::vector &landmark_ids);
void SetEstimatedPose(const Matrix &pose);
+ void SetEstimatedInversePose(const Matrix &pose, const std::vector &landmark_ids);
void SetEstimatedInversePose(const Matrix &pose);
void CompressImages();
@@ -56,11 +61,15 @@ class Frame
double timeSec_;
camera::CameraModel<> &cameraModel_;
+ std::unordered_set estLandmarkIds_;
+
bool hasPose_;
bool hasDepth_;
bool hasPoseEstimate_{false};
bool isCompressed_{false};
+
+ static int lastFrameId_;
};
}
diff --git a/src/data/landmark.cc b/src/data/landmark.cc
index 7eb0a5c..a2dbff2 100644
--- a/src/data/landmark.cc
+++ b/src/data/landmark.cc
@@ -5,7 +5,10 @@ namespace omni_slam
namespace data
{
+int Landmark::lastLandmarkId_ = 0;
+
Landmark::Landmark()
+ : id_(lastLandmarkId_++)
{
}
@@ -21,7 +24,7 @@ void Landmark::AddObservation(Feature obs, bool compute_gnd)
if (obs.HasEstimatedWorldPoint())
{
posEstimate_ = obs.GetEstimatedWorldPoint();
- obsForEst_.push_back(obs);
+ estFrameIds_.insert(obs.GetFrame().GetID());
hasPosEstimate_ = true;
}
}
@@ -38,11 +41,6 @@ const std::vector& Landmark::GetObservations() const
return obs_;
}
-const std::vector& Landmark::GetObservationsForEstimate() const
-{
- return obsForEst_;
-}
-
std::vector& Landmark::GetObservations()
{
return obs_;
@@ -83,18 +81,24 @@ const Feature* Landmark::GetObservationByFrameID(const int frame_id) const
return &obs_[idToIndex_.at(frame_id)];
}
+void Landmark::SetEstimatedPosition(const Vector3d &pos, const std::vector &frame_ids)
+{
+ posEstimate_ = pos;
+ estFrameIds_ = std::unordered_set(frame_ids.begin(), frame_ids.end());
+ hasPosEstimate_ = true;
+}
+
void Landmark::SetEstimatedPosition(const Vector3d &pos)
{
posEstimate_ = pos;
- obsForEst_.clear();
- obsForEst_.reserve(obs_.size());
- for (data::Feature &feat : obs_)
- {
- obsForEst_.push_back(feat);
- }
hasPosEstimate_ = true;
}
+const int Landmark::GetID() const
+{
+ return id_;
+}
+
Vector3d Landmark::GetGroundTruth() const
{
return groundTruth_;
@@ -115,5 +119,10 @@ bool Landmark::HasEstimatedPosition() const
return hasPosEstimate_;
}
+bool Landmark::IsEstimatedByFrame(const int frame_id) const
+{
+ return estFrameIds_.find(frame_id) != estFrameIds_.end();
+}
+
}
}
diff --git a/src/data/landmark.h b/src/data/landmark.h
index f68c71e..db5b9e4 100644
--- a/src/data/landmark.h
+++ b/src/data/landmark.h
@@ -3,6 +3,7 @@
#include "feature.h"
#include
+#include
#include
using namespace Eigen;
@@ -19,26 +20,31 @@ class Landmark
void AddObservation(Feature obs, bool compute_gnd = true);
const std::vector& GetObservations() const;
std::vector& GetObservations();
- const std::vector& GetObservationsForEstimate() const;
bool IsObservedInFrame(const int frame_id) const;
const int GetFirstFrameID() const;
const int GetNumObservations() const;
const Feature* GetObservationByFrameID(const int frame_id) const;
+ void SetEstimatedPosition(const Vector3d &pos, const std::vector &frame_ids);
void SetEstimatedPosition(const Vector3d &pos);
+ const int GetID() const;
Vector3d GetGroundTruth() const;
Vector3d GetEstimatedPosition() const;
bool HasGroundTruth() const;
bool HasEstimatedPosition() const;
+ bool IsEstimatedByFrame(const int frame_id) const;
private:
+ const int id_;
std::vector obs_;
- std::vector obsForEst_;
+ std::unordered_set estFrameIds_;
std::map idToIndex_;
Vector3d groundTruth_;
Vector3d posEstimate_;
bool hasGroundTruth_{false};
bool hasPosEstimate_{false};
+
+ static int lastLandmarkId_;
};
}
diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc
index fe0e5cf..7de3d35 100644
--- a/src/odometry/pnp.cc
+++ b/src/odometry/pnp.cc
@@ -16,9 +16,10 @@ namespace omni_slam
namespace odometry
{
-PNP::PNP(int ransac_iterations, double reprojection_threshold)
+PNP::PNP(int ransac_iterations, double reprojection_threshold, int num_refine_threads)
: ransacIterations_(ransac_iterations),
- reprojThreshold_(reprojection_threshold)
+ reprojThreshold_(reprojection_threshold),
+ numRefineThreads_(num_refine_threads)
{
}
@@ -28,6 +29,7 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram
std::vector yns;
std::vector ys;
std::vector features;
+ std::map indexToId;
for (const data::Landmark &landmark : landmarks)
{
if (landmark.IsObservedInFrame(frame.GetID()))
@@ -50,6 +52,7 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram
yns.push_back(pix);
ys.push_back(feat->GetBearing());
features.push_back(feat);
+ indexToId[xs.size() - 1] = landmark.GetID();
}
}
Matrix pose;
@@ -59,13 +62,20 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram
{
Refine(xs, features, indices, pose);
}
- frame.SetEstimatedInversePose(pose);
+ std::vector inlierIds;
+ inlierIds.reserve(indices.size());
+ for (int inx : indices)
+ {
+ inlierIds.push_back(indexToId[inx]);
+ }
+ frame.SetEstimatedInversePose(pose, inlierIds);
return inliers;
}
int PNP::RANSAC(const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, Matrix &pose) const
{
int maxInliers = 0;
+ #pragma omp parallel for
for (int i = 0; i < ransacIterations_; i++)
{
std::random_device rd;
@@ -87,10 +97,13 @@ int PNP::RANSAC(const std::vector &xs, const std::vector &ys
}
int inliers = GetInlierIndices(xs, yns, iterPose, camera_model).size();
- if (inliers > maxInliers)
+ #pragma omp critical
{
- maxInliers = inliers;
- pose = iterPose;
+ if (inliers > maxInliers)
+ {
+ maxInliers = inliers;
+ pose = iterPose;
+ }
}
}
return maxInliers;
@@ -133,6 +146,7 @@ bool PNP::Refine(const std::vector &xs, const std::vector &landmarks, data::Frame &frame) const;
private:
@@ -28,6 +28,7 @@ class PNP
int ransacIterations_;
double reprojThreshold_;
+ int numRefineThreads_;
};
}
diff --git a/src/optimization/bundle_adjuster.cc b/src/optimization/bundle_adjuster.cc
index 39c2506..5fc702b 100644
--- a/src/optimization/bundle_adjuster.cc
+++ b/src/optimization/bundle_adjuster.cc
@@ -10,12 +10,15 @@ namespace omni_slam
namespace optimization
{
-BundleAdjuster::BundleAdjuster(int max_iterations, bool log)
+BundleAdjuster::BundleAdjuster(int max_iterations, double loss_coeff, int num_threads, bool log)
+ : lossCoeff_(loss_coeff)
{
problem_.reset(new ceres::Problem());
solverOptions_.max_num_iterations = max_iterations;
solverOptions_.linear_solver_type = ceres::ITERATIVE_SCHUR;
solverOptions_.preconditioner_type = ceres::SCHUR_JACOBI;
+ solverOptions_.num_threads = num_threads;
+ solverOptions_.use_inner_iterations = true;
solverOptions_.minimizer_progress_to_stdout = log;
solverOptions_.logging_type = log ? ceres::PER_MINIMIZER_ITERATION : ceres::SILENT;
}
@@ -26,7 +29,7 @@ bool BundleAdjuster::Optimize(std::vector &landmarks)
landmarkEstimates.reserve(3 * landmarks.size());
std::map, std::vector>> framePoses;
std::map estFrames;
- ceres::LossFunction *loss_function = new ceres::HuberLoss(1.0);
+ ceres::LossFunction *loss_function = new ceres::HuberLoss(lossCoeff_);
for (const data::Landmark &landmark : landmarks)
{
bool hasEstCameraPoses = false;
@@ -58,8 +61,7 @@ bool BundleAdjuster::Optimize(std::vector &landmarks)
{
continue;
}
- const std::vector obs = landmark.HasEstimatedPosition() ? landmark.GetObservationsForEstimate() : landmark.GetObservations();
- for (const data::Feature &feature : obs)
+ for (const data::Feature &feature : landmark.GetObservations())
{
if (!feature.GetFrame().HasEstimatedPose() && feature.GetFrame().HasPose())
{
@@ -67,6 +69,10 @@ bool BundleAdjuster::Optimize(std::vector &landmarks)
{
continue;
}
+ if (!landmark.IsEstimatedByFrame(feature.GetFrame().GetID()))
+ {
+ continue;
+ }
if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
{
const Matrix &pose = feature.GetFrame().GetInversePose();
@@ -82,6 +88,10 @@ bool BundleAdjuster::Optimize(std::vector &landmarks)
}
else if (feature.GetFrame().HasEstimatedPose())
{
+ if (!(feature.GetFrame().IsEstimatedByLandmark(landmark.GetID()) || (landmark.HasEstimatedPosition() && landmark.IsEstimatedByFrame(feature.GetFrame().GetID()))))
+ {
+ continue;
+ }
if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
{
const Matrix &pose = feature.GetFrame().GetInversePose();
@@ -115,10 +125,6 @@ bool BundleAdjuster::Optimize(std::vector &landmarks)
}
}
- if (solverOptions_.minimizer_progress_to_stdout)
- {
- std::cout << "Running bundle adjustment..." << std::endl;
- }
ceres::Solver::Summary summary;
ceres::Solve(solverOptions_, problem_.get(), &summary);
if (solverOptions_.minimizer_progress_to_stdout)
diff --git a/src/optimization/bundle_adjuster.h b/src/optimization/bundle_adjuster.h
index 1ffe7a7..3713a82 100644
--- a/src/optimization/bundle_adjuster.h
+++ b/src/optimization/bundle_adjuster.h
@@ -12,12 +12,15 @@ namespace optimization
class BundleAdjuster
{
public:
- BundleAdjuster(int max_iterations = 500, bool log = false);
+ BundleAdjuster(int max_iterations = 500, double loss_coeff = 0.1, int num_threads = 1, bool log = false);
bool Optimize(std::vector &landmarks);
+private:
std::unique_ptr problem_;
ceres::Solver::Options solverOptions_;
+
+ double lossCoeff_;
};
}
diff --git a/src/reconstruction/triangulator.cc b/src/reconstruction/triangulator.cc
index 4172a7c..4ba1256 100644
--- a/src/reconstruction/triangulator.cc
+++ b/src/reconstruction/triangulator.cc
@@ -25,7 +25,13 @@ int Triangulator::Triangulate(std::vector &landmarks) const
{
if (CheckReprojectionErrors(landmark.GetObservations(), point))
{
- landmark.SetEstimatedPosition(point);
+ std::vector frameIds;
+ frameIds.reserve(landmark.GetObservations().size());
+ for (data::Feature &obs : landmark.GetObservations())
+ {
+ frameIds.push_back(obs.GetFrame().GetID());
+ }
+ landmark.SetEstimatedPosition(point, frameIds);
numSuccess++;
}
}
diff --git a/src/ros/eval_base.cc b/src/ros/eval_base.cc
index 4c7426e..5b3e67c 100644
--- a/src/ros/eval_base.cc
+++ b/src/ros/eval_base.cc
@@ -75,11 +75,9 @@ void EvalBase::FrameCallback(const sensor_msgs::ImageConstPtr &image, const sens
cv::Mat depthFloatImg;
cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535);
- ProcessFrame(std::unique_ptr(new data::Frame(frameNum_, monoImg, depthFloatImg, posemat, pose->header.stamp.toSec(), *cameraModel_)));
+ ProcessFrame(std::unique_ptr(new data::Frame(monoImg, depthFloatImg, posemat, pose->header.stamp.toSec(), *cameraModel_)));
Visualize(cvImage);
-
- frameNum_++;
}
void EvalBase::Finish()
diff --git a/src/ros/eval_base.h b/src/ros/eval_base.h
index d924792..4b5c5dc 100644
--- a/src/ros/eval_base.h
+++ b/src/ros/eval_base.h
@@ -40,8 +40,6 @@ class EvalBase
std::unique_ptr> cameraModel_;
- int frameNum_{0};
-
private:
void FrameCallback(const sensor_msgs::ImageConstPtr &image, const sensor_msgs::ImageConstPtr &depth_image, const geometry_msgs::PoseStamped::ConstPtr &pose);
diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc
index 0dc5b56..b6f492e 100644
--- a/src/ros/odometry_eval.cc
+++ b/src/ros/odometry_eval.cc
@@ -5,6 +5,7 @@
#include "module/tracking_module.h"
#include "geometry_msgs/PoseStamped.h"
+#include "nav_msgs/Path.h"
using namespace std;
@@ -19,15 +20,19 @@ OdometryEval::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle
double reprojThresh;
int iterations;
int baMaxIter;
+ double baLossCoeff;
bool logCeres;
+ int numCeresThreads;
nhp_.param("pnp_inlier_threshold", reprojThresh, 10.);
nhp_.param("pnp_iterations", iterations, 1000);
nhp_.param("bundle_adjustment_max_iterations", baMaxIter, 500);
+ nhp_.param("bundle_adjustment_loss_coefficient", baLossCoeff, 0.1);
nhp_.param("bundle_adjustment_logging", logCeres, false);
+ nhp_.param("bundle_adjustment_num_threads", numCeresThreads, 1);
- unique_ptr pnp(new odometry::PNP(iterations, reprojThresh));
- unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, logCeres));
+ unique_ptr pnp(new odometry::PNP(iterations, reprojThresh, numCeresThreads));
+ unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres));
odometryModule_.reset(new module::OdometryModule(pnp, bundleAdjuster));
}
@@ -37,10 +42,16 @@ void OdometryEval::InitPublishers()
TrackingEval::InitPublishers();
string outputTopic;
string outputGndTopic;
+ string outputPathTopic;
+ string outputPathGndTopic;
nhp_.param("odometry_estimate_topic", outputTopic, string("/omni_slam/odometry"));
nhp_.param("odometry_ground_truth_topic", outputGndTopic, string("/omni_slam/odometry_truth"));
+ nhp_.param("path_estimate_topic", outputPathTopic, string("/omni_slam/odometry_path"));
+ nhp_.param("path_ground_truth_topic", outputPathGndTopic, string("/omni_slam/odometry_path_truth"));
odometryPublisher_ = nh_.advertise(outputTopic, 2);
odometryGndPublisher_ = nh_.advertise(outputGndTopic, 2);
+ pathPublisher_ = nh_.advertise(outputPathTopic, 2);
+ pathGndPublisher_ = nh_.advertise(outputPathGndTopic, 2);
}
void OdometryEval::ProcessFrame(unique_ptr &&frame)
@@ -52,7 +63,9 @@ void OdometryEval::ProcessFrame(unique_ptr &&frame)
void OdometryEval::Finish()
{
+ ROS_INFO("Performing bundle adjustment...");
odometryModule_->BundleAdjust(trackingModule_->GetLandmarks());
+ PublishOdometry();
}
void OdometryEval::GetResultsData(std::map>> &data)
@@ -63,8 +76,12 @@ void OdometryEval::GetResultsData(std::mapGetFrames().back()->HasEstimatedPose())
{
@@ -77,6 +94,7 @@ void OdometryEval::Visualize(cv_bridge::CvImagePtr &base_img)
poseMsg.pose.orientation.y = quat.normalized().y();
poseMsg.pose.orientation.z = quat.normalized().z();
poseMsg.pose.orientation.w = quat.normalized().w();
+ poseMsg.header.stamp = ::ros::Time(trackingModule_->GetFrames().back()->GetTime());
odometryPublisher_.publish(poseMsg);
}
const Matrix &poseGnd = trackingModule_->GetFrames().back()->GetPose();
@@ -88,7 +106,52 @@ void OdometryEval::Visualize(cv_bridge::CvImagePtr &base_img)
poseMsg.pose.orientation.y = quatGnd.normalized().y();
poseMsg.pose.orientation.z = quatGnd.normalized().z();
poseMsg.pose.orientation.w = quatGnd.normalized().w();
+ poseMsg.header.stamp = ::ros::Time(trackingModule_->GetFrames().back()->GetTime());
odometryGndPublisher_.publish(poseMsg);
+
+ nav_msgs::Path path;
+ path.header.stamp = ::ros::Time::now();
+ path.header.frame_id = "map";
+ nav_msgs::Path pathGnd;
+ pathGnd.header.stamp = ::ros::Time::now();
+ pathGnd.header.frame_id = "map";
+ for (const std::unique_ptr &frame : trackingModule_->GetFrames())
+ {
+ if (frame->HasEstimatedPose())
+ {
+ geometry_msgs::PoseStamped poseMsg;
+ poseMsg.header.stamp = ::ros::Time(frame->GetTime());
+ poseMsg.header.frame_id = "map";
+ const Matrix &pose = frame->GetEstimatedPose();
+ poseMsg.pose.position.x = pose(0, 3);
+ poseMsg.pose.position.y = pose(1, 3);
+ poseMsg.pose.position.z = pose(2, 3);
+ Quaterniond quat(pose.block<3, 3>(0, 0));
+ poseMsg.pose.orientation.x = quat.normalized().x();
+ poseMsg.pose.orientation.y = quat.normalized().y();
+ poseMsg.pose.orientation.z = quat.normalized().z();
+ poseMsg.pose.orientation.w = quat.normalized().w();
+ path.poses.push_back(poseMsg);
+ }
+ if (frame->HasPose())
+ {
+ geometry_msgs::PoseStamped poseMsg;
+ poseMsg.header.stamp = ::ros::Time(frame->GetTime());
+ poseMsg.header.frame_id = "map";
+ const Matrix &pose = frame->GetPose();
+ poseMsg.pose.position.x = pose(0, 3);
+ poseMsg.pose.position.y = pose(1, 3);
+ poseMsg.pose.position.z = pose(2, 3);
+ Quaterniond quat(pose.block<3, 3>(0, 0));
+ poseMsg.pose.orientation.x = quat.normalized().x();
+ poseMsg.pose.orientation.y = quat.normalized().y();
+ poseMsg.pose.orientation.z = quat.normalized().z();
+ poseMsg.pose.orientation.w = quat.normalized().w();
+ pathGnd.poses.push_back(poseMsg);
+ }
+ }
+ pathPublisher_.publish(path);
+ pathGndPublisher_.publish(pathGnd);
}
}
diff --git a/src/ros/odometry_eval.h b/src/ros/odometry_eval.h
index 2de3e2d..6ca98fc 100644
--- a/src/ros/odometry_eval.h
+++ b/src/ros/odometry_eval.h
@@ -25,11 +25,14 @@ class OdometryEval : public TrackingEval
void Finish();
void GetResultsData(std::map>> &data);
void Visualize(cv_bridge::CvImagePtr &base_img);
+ void PublishOdometry();
std::unique_ptr odometryModule_;
::ros::Publisher odometryPublisher_;
::ros::Publisher odometryGndPublisher_;
+ ::ros::Publisher pathPublisher_;
+ ::ros::Publisher pathGndPublisher_;
};
}
diff --git a/src/ros/reconstruction_eval.cc b/src/ros/reconstruction_eval.cc
index 0e2f9be..2260da7 100644
--- a/src/ros/reconstruction_eval.cc
+++ b/src/ros/reconstruction_eval.cc
@@ -20,15 +20,19 @@ ReconstructionEval::ReconstructionEval(const ::ros::NodeHandle &nh, const ::ros:
double maxReprojError;
double minTriAngle;
int baMaxIter;
+ double baLossCoeff;
bool logCeres;
+ int numCeresThreads;
nhp_.param("max_reprojection_error", maxReprojError, 0.0);
nhp_.param("min_triangulation_angle", minTriAngle, 1.0);
nhp_.param("bundle_adjustment_max_iterations", baMaxIter, 500);
+ nhp_.param("bundle_adjustment_loss_coefficient", baLossCoeff, 0.1);
nhp_.param("bundle_adjustment_logging", logCeres, false);
+ nhp_.param("bundle_adjustment_num_threads", numCeresThreads, 1);
unique_ptr triangulator(new reconstruction::Triangulator(maxReprojError, minTriAngle));
- unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, logCeres));
+ unique_ptr bundleAdjuster(new optimization::BundleAdjuster(baMaxIter, baLossCoeff, numCeresThreads, logCeres));
reconstructionModule_.reset(new module::ReconstructionModule(triangulator, bundleAdjuster));
}
@@ -50,6 +54,7 @@ void ReconstructionEval::ProcessFrame(unique_ptr &&frame)
void ReconstructionEval::Finish()
{
+ ROS_INFO("Performing bundle adjustment...");
reconstructionModule_->BundleAdjust(trackingModule_->GetLandmarks());
pcl::PointCloud cloud;