Skip to content

Commit

Permalink
fix BA
Browse files Browse the repository at this point in the history
  • Loading branch information
@raphaelchang
raphaelchang committed Sep 19, 2019
1 parent 13e38e0 commit de601d9
Show file tree
Hide file tree
Showing 14 changed files with 97 additions and 22 deletions.
8 changes: 8 additions & 0 deletions src/data/frame.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix
hasPose_ = true;
hasDepth_ = true;
hasStereo_ = true;
poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
Expand All @@ -35,6 +36,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo
hasPose_ = false;
hasDepth_ = false;
hasStereo_ = true;
pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
Expand All @@ -47,6 +49,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraMo
hasPose_ = false;
hasDepth_ = true;
hasStereo_ = false;
pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
Expand All @@ -61,6 +64,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix
hasPose_ = false;
hasDepth_ = true;
hasStereo_ = true;
pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, double time, camera::CameraModel<> &camera_model)
Expand All @@ -74,6 +78,7 @@ Frame::Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, double time, camera::C
hasPose_ = true;
hasDepth_ = false;
hasStereo_ = false;
poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
Expand All @@ -89,6 +94,7 @@ Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &pose,
hasPose_ = true;
hasDepth_ = false;
hasStereo_ = true;
poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
Expand All @@ -103,6 +109,7 @@ Frame::Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, cv::Mat &depth_image,
hasPose_ = true;
hasDepth_ = true;
hasStereo_ = false;
poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
Expand All @@ -113,6 +120,7 @@ Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
hasPose_ = false;
hasDepth_ = false;
hasStereo_ = false;
pose_ = invPose_ = poseEstimate_ = invPoseEstimate_ = util::TFUtil::IdentityPoseMatrix<double>();
}

Frame::Frame(const Frame &other)
Expand Down
4 changes: 0 additions & 4 deletions src/module/odometry_module.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,6 @@ void OdometryModule::Update(std::vector<data::Landmark> &landmarks, data::Frame
{
if (landmarks.size() == 0)
{
if (frame.HasPose())
{
frame.SetEstimatedPose(frame.GetPose());
}
return;
}
pnp_->Compute(landmarks, frame);
Expand Down
14 changes: 10 additions & 4 deletions src/module/stereo_module.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,15 @@ void StereoModule::Update(data::Frame &frame, std::vector<data::Landmark> &landm
{
Matrix<double, 3, 4> framePose = frame.HasEstimatedPose() ? frame.GetEstimatedPose() : frame.GetPose();
double depth = (landmark.GetEstimatedPosition() - framePose.block<3, 1>(0, 3)).norm();
double depthGnd = (landmark.GetGroundTruth() - frame.GetPose().block<3, 1>(0, 3)).norm();
visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth, depthGnd);
if (frame.HasPose() && landmark.HasGroundTruth())
{
double depthGnd = (landmark.GetGroundTruth() - frame.GetPose().block<3, 1>(0, 3)).norm();
visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth, std::abs(depth - depthGnd) / depthGnd);
}
else
{
visualization_.AddMatch(feat1->GetKeypoint().pt, feat2->GetKeypoint().pt, depth, 0);
}
}
}
frameNum_++;
Expand All @@ -58,9 +65,8 @@ void StereoModule::Visualization::Init(cv::Size img_size)
curDepth_ = cv::Mat::zeros(img_size, CV_8UC3);
}

void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double depthGnd)
void StereoModule::Visualization::AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double err)
{
double err = std::abs(depth - depthGnd) / depthGnd;
err = min(err, 1.0);
depth = min(depth, maxDepth_);
cv::circle(curMask_, pt1, 3, cv::Scalar(255, 0, 0), -1);
Expand Down
2 changes: 1 addition & 1 deletion src/module/stereo_module.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ class StereoModule
{
public:
void Init(cv::Size img_size);
void AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double depthGnd);
void AddMatch(cv::Point2f pt1, cv::Point2f pt2, double depth, double err);
void Draw(cv::Mat &img, const cv::Mat &stereo_img);

private:
Expand Down
13 changes: 12 additions & 1 deletion src/module/tracking_module.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,14 +79,14 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
int tinx = min((int)(ti - ts_.begin()), (int)(ts_.size() - 1)) - 1;
regionCount_[{rinx, tinx}]++;

const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID());
Vector2d pixelGnd;
if (frames_.back()->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(frames_.back()->GetInversePose(), landmark.GetGroundTruth())), pixelGnd))
{
Vector2d pixel;
pixel << obs->GetKeypoint().pt.x, obs->GetKeypoint().pt.y;
double error = (pixel - pixelGnd).norm();

const data::Feature *obsPrev = landmark.GetObservationByFrameID((*next(frames_.rbegin()))->GetID());
visualization_.AddTrack(cv::Point2f(pixelGnd(0), pixelGnd(1)), obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, error, i);

double xg = pixelGnd(0) - frames_.back()->GetImage().cols / 2. + 0.5;
Expand All @@ -95,6 +95,10 @@ void TrackingModule::Update(std::unique_ptr<data::Frame> &frame)
stats_.radialErrors.emplace_back(vector<double>{rg, error});
stats_.frameErrors.emplace_back(vector<double>{(double)landmark.GetNumObservations() - 1, (double)i, rg, error});
}
else if (!frames_.back()->HasPose())
{
visualization_.AddTrack(obsPrev->GetKeypoint().pt, obs->GetKeypoint().pt, i);
}
stats_.trackLengths[i]++;
numGood++;
}
Expand Down Expand Up @@ -181,6 +185,13 @@ void TrackingModule::Visualization::AddTrack(cv::Point2f gnd, cv::Point2f prev,
cv::circle(curMask_, gnd, 3, colors_[index % colors_.size()], -1);
}

void TrackingModule::Visualization::AddTrack(cv::Point2f prev, cv::Point2f cur, int index)
{
cv::Scalar color(255, 0, 0);
cv::line(visMask_, prev, cur, color, 1);
cv::circle(curMask_, cur, 1, color, -1);
}

void TrackingModule::Visualization::Draw(cv::Mat &img)
{
if (img.channels() == 1)
Expand Down
1 change: 1 addition & 0 deletions src/module/tracking_module.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,7 @@ class TrackingModule
public:
void Init(cv::Size img_size, int num_colors);
void AddTrack(cv::Point2f gnd, cv::Point2f prev, cv::Point2f cur, double error, int index);
void AddTrack(cv::Point2f prev, cv::Point2f cur, int index);
void Draw(cv::Mat &img);

private:
Expand Down
1 change: 1 addition & 0 deletions src/odometry/pnp.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ int PNP::Compute(const std::vector<data::Landmark> &landmarks, data::Frame &fram
}
else
{
i++;
continue;
}
const data::Feature *feat = landmark.GetObservationByFrameID(frame.GetID());
Expand Down
4 changes: 2 additions & 2 deletions src/optimization/bundle_adjuster.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
bool hasEstCameraPoses = false;
for (const data::Feature &feature : landmark.GetObservations())
{
if (feature.GetFrame().HasEstimatedPose())
if (feature.GetFrame().HasEstimatedPose() && feature.GetFrame().IsEstimatedByLandmark(landmark.GetID()))
{
hasEstCameraPoses = true;
}
Expand Down Expand Up @@ -94,7 +94,7 @@ bool BundleAdjuster::Optimize(std::vector<data::Landmark> &landmarks)
}
if (framePoses.find(feature.GetFrame().GetID()) == framePoses.end())
{
const Matrix<double, 3, 4> &pose = feature.GetFrame().GetInversePose();
const Matrix<double, 3, 4> &pose = feature.GetFrame().GetEstimatedInversePose();
Quaterniond quat(pose.block<3, 3>(0, 0));
quat.normalize();
const Vector3d &t = pose.block<3, 1>(0, 3);
Expand Down
27 changes: 26 additions & 1 deletion src/ros/eval_base.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@
#include "util/tf_util.h"
#include "util/hdf_file.h"

#define USE_GROUND_TRUTH

using namespace Eigen;

namespace omni_slam
Expand Down Expand Up @@ -103,8 +105,19 @@ void EvalBase<Stereo>::FrameCallback(const sensor_msgs::ImageConstPtr &image, co
cv::Mat depthFloatImg;
cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535);

#ifdef USE_GROUND_TRUTH
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, posemat, depthFloatImg, pose->header.stamp.toSec(), *cameraModel_)));

#else
if (first_)
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, posemat, pose->header.stamp.toSec(), *cameraModel_)));
first_ = false;
}
else
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, pose->header.stamp.toSec(), *cameraModel_)));
}
#endif
Visualize(cvImage);
}

Expand Down Expand Up @@ -136,7 +149,19 @@ void EvalBase<Stereo>::FrameCallback(const sensor_msgs::ImageConstPtr &image, co
cv::Mat depthFloatImg;
cvDepthImage->image.convertTo(depthFloatImg, CV_64FC1, 500. / 65535);

#ifdef USE_GROUND_TRUTH
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, depthFloatImg, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
#else
if (first_)
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, posemat, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
first_ = false;
}
else
{
ProcessFrame(std::unique_ptr<data::Frame>(new data::Frame(monoImg, monoImg2, stereoPose_, pose->header.stamp.toSec(), *cameraModel_)));
}
#endif

Visualize(cvImage, cvStereoImage);
}
Expand Down
2 changes: 2 additions & 0 deletions src/ros/eval_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,8 @@ class EvalBase
std::string poseTopic_;
std::map<std::string, double> cameraParams_;
Matrix<double, 3, 4> stereoPose_;

bool first_{true};
};

}
Expand Down
12 changes: 8 additions & 4 deletions src/ros/odometry_eval.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@ void OdometryEval<Stereo>::ProcessFrame(unique_ptr<data::Frame> &&frame)
this->trackingModule_->Update(frame);
odometryModule_->Update(this->trackingModule_->GetLandmarks(), *this->trackingModule_->GetFrames().back());
this->trackingModule_->Redetect();

this->visualized_ = false;
}

template <bool Stereo>
Expand Down Expand Up @@ -90,9 +92,9 @@ void OdometryEval<Stereo>::PublishOdometry()
{
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.frame_id = "map";
if (this->trackingModule_->GetFrames().back()->HasEstimatedPose())
if (this->trackingModule_->GetFrames().back()->HasEstimatedPose() || this->trackingModule_->GetFrames().size() == 1)
{
const Matrix<double, 3, 4> &pose = this->trackingModule_->GetFrames().back()->GetEstimatedPose();
const Matrix<double, 3, 4> &pose = (this->trackingModule_->GetFrames().size() == 1 && !this->trackingModule_->GetFrames().back()->HasEstimatedPose()) ? this->trackingModule_->GetFrames().back()->GetPose() : this->trackingModule_->GetFrames().back()->GetEstimatedPose();
poseMsg.pose.position.x = pose(0, 3);
poseMsg.pose.position.y = pose(1, 3);
poseMsg.pose.position.z = pose(2, 3);
Expand Down Expand Up @@ -122,14 +124,15 @@ void OdometryEval<Stereo>::PublishOdometry()
nav_msgs::Path pathGnd;
pathGnd.header.stamp = ::ros::Time::now();
pathGnd.header.frame_id = "map";
bool first = true;
for (const std::unique_ptr<data::Frame> &frame : this->trackingModule_->GetFrames())
{
if (frame->HasEstimatedPose())
if (frame->HasEstimatedPose() || first)
{
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ::ros::Time(frame->GetTime());
poseMsg.header.frame_id = "map";
const Matrix<double, 3, 4> &pose = frame->GetEstimatedPose();
const Matrix<double, 3, 4> &pose = (first && !frame->HasEstimatedPose()) ? frame->GetPose() : frame->GetEstimatedPose();
poseMsg.pose.position.x = pose(0, 3);
poseMsg.pose.position.y = pose(1, 3);
poseMsg.pose.position.z = pose(2, 3);
Expand All @@ -156,6 +159,7 @@ void OdometryEval<Stereo>::PublishOdometry()
poseMsg.pose.orientation.w = quat.normalized().w();
pathGnd.poses.push_back(poseMsg);
}
first = false;
}
pathPublisher_.publish(path);
pathGndPublisher_.publish(pathGnd);
Expand Down
2 changes: 2 additions & 0 deletions src/ros/reconstruction_eval.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,8 @@ void ReconstructionEval<Stereo>::ProcessFrame(unique_ptr<data::Frame> &&frame)
this->trackingModule_->Update(frame);
reconstructionModule_->Update(this->trackingModule_->GetLandmarks());
this->trackingModule_->Redetect();

this->visualized_ = false;
}

template <bool Stereo>
Expand Down
20 changes: 15 additions & 5 deletions src/stereo/stereo_matcher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,19 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
{
return 0;
}
Matrix<double, 3, 4> framePose;
if (frame.HasEstimatedPose())
{
framePose = frame.GetEstimatedPose();
}
else if (frame.HasPose())
{
framePose = frame.GetPose();
}
else
{
return 0;
}
std::vector<cv::KeyPoint> pointsToMatch;
std::vector<Vector3d> bearings1;
std::vector<int> origInx;
Expand Down Expand Up @@ -45,9 +58,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
std::vector<int> matchedIndices;
FindMatches(frame.GetImage(), frame.GetStereoImage(), pointsToMatch, matchedPoints, matchedIndices);

Matrix<double, 3, 4> I;
I.block<3, 3>(0, 0) = Matrix3d::Identity();
I.block<3, 1>(0, 3) = Vector3d::Zero();
Matrix<double, 3, 4> I = util::TFUtil::IdentityPoseMatrix<double>();
Matrix3d E = util::TFUtil::GetEssentialMatrixFromPoses(I, frame.GetStereoPose());
int good = 0;
#pragma omp parallel for reduction(+:good)
Expand All @@ -66,8 +77,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
double epiErr2 = std::abs(bearing2.transpose() * epiplane2);
if (epiErr1 < epipolarThresh_ && epiErr2 < epipolarThresh_)
{
Matrix<double, 3, 4> framePose = frame.HasEstimatedPose() ? frame.GetEstimatedPose() : frame.GetPose();
landmarks[origInx[inx]].SetEstimatedPosition(util::TFUtil::TransformPoint(framePose, TriangulateDLT(bearing1, bearing2, I, frame.GetStereoPose())));
landmarks[origInx[inx]].SetEstimatedPosition(util::TFUtil::TransformPoint(framePose, TriangulateDLT(bearing1, bearing2, I, frame.GetStereoPose())), std::vector<int>({frame.GetID()}));
landmarks[origInx[inx]].AddStereoObservation(feat);
good++;
}
Expand Down
9 changes: 9 additions & 0 deletions src/util/tf_util.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,15 @@ inline Matrix<T, 3, 3> GetEssentialMatrixFromPoses(Matrix<T, 3, 4> pose1, Matrix
return tx * GetRotationFromPoseMatrix(rel);
}

template <typename T>
inline Matrix<T, 3, 4> IdentityPoseMatrix()
{
Matrix<T, 3, 4> I;
I.template block<3, 3>(0, 0) = Matrix3d::Identity();
I.template block<3, 1>(0, 3) = Vector3d::Zero();
return I;
}

}
}
}
Expand Down

0 comments on commit de601d9

Please sign in to comment.