Skip to content

Commit

Permalink
coordinate frames
Browse files Browse the repository at this point in the history
  • Loading branch information
@raphaelchang
raphaelchang committed Sep 20, 2019
1 parent 3bb3dfc commit 0b2193a
Show file tree
Hide file tree
Showing 10 changed files with 82 additions and 17 deletions.
1 change: 1 addition & 0 deletions launch/odometry_eval.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
<param name="path_estimate_topic" value="/omni_slam/odometry_path" />
<param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
<param name="output_frame" value="map" />
<param name="rate" value="$(arg rate)" />
<rosparam subst_value="true">
camera_model: '$(arg camera_model)'
Expand Down
1 change: 1 addition & 0 deletions launch/reconstruction_eval.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
<param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
<param name="tracked_image_topic" value="/omni_slam/tracked" />
<param name="point_cloud_topic" value="/omni_slam/reconstructed" />
<param name="output_frame" value="map" />
<param name="rate" value="$(arg rate)" />
<rosparam subst_value="true">
camera_model: '$(arg camera_model)'
Expand Down
1 change: 1 addition & 0 deletions launch/slam_eval.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
<param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
<param name="point_cloud_topic" value="/omni_slam/reconstructed" />
<param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
<param name="output_frame" value="map" />
<param name="rate" value="$(arg rate)" />
<rosparam subst_value="true">
camera_model: '$(arg camera_model)'
Expand Down
56 changes: 56 additions & 0 deletions launch/slam_eval_kitti.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,56 @@
<launch>
<arg name="bag_file" default="" />
<arg name="results_file" default="$(arg bag_file).slam.hdf5" />
<arg name="camera_model" default="perspective" />
<arg name="camera_params" default="{fx: 721.5377, fy: 721.5377, cx: 609.5593, cy: 172.854}" />
<arg name="rate" default="1" />
<node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
<param name="bag_file" value="$(arg bag_file)" />
<param name="results_file" value="$(arg results_file)" />
<param name="image_topic" value="cam02/image_raw" />
<param name="stereo_image_topic" value="cam03/image_raw" />
<param name="depth_image_topic" value="cam00/image_raw" />
<param name="pose_topic" value="pose_imu" />
<param name="tracked_image_topic" value="/omni_slam/tracked" />
<param name="odometry_estimate_topic" value="/omni_slam/odometry" />
<param name="odometry_ground_truth_topic" value="/omni_slam/odometry_truth" />
<param name="path_estimate_topic" value="/omni_slam/odometry_path" />
<param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
<param name="point_cloud_topic" value="/omni_slam/reconstructed" />
<param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
<param name="output_frame" value="imu" />
<param name="rate" value="$(arg rate)" />
<rosparam subst_value="true">
camera_model: '$(arg camera_model)'
camera_parameters: $(arg camera_params)
detector_type: 'GFTT'
detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
tracker_window_size: 128
tracker_num_scales: 4
tracker_checker_epipolar_threshold: 0.008
tracker_checker_iterations: 1000
tracker_delta_pixel_error_threshold: 0.0
tracker_error_threshold: 20.0
min_features_per_region: 100
max_features_per_region: 1000
pnp_inlier_threshold: 3.0
pnp_iterations: 3000
max_reprojection_error: 2.0
min_triangulation_angle: 5.0
bundle_adjustment_max_iterations: 1000
bundle_adjustment_loss_coefficient: 0.05
bundle_adjustment_logging: true
bundle_adjustment_num_threads: 20
local_bundle_adjustment_window: 0
local_bundle_adjustment_interval: 0
stereo_matcher_window_size: 256
stereo_matcher_num_scales: 5
stereo_matcher_error_threshold: 20
stereo_matcher_epipolar_threshold: 0.008
stereo_tf_t: [0.533, 0.005, -0.001]
stereo_tf_r: [0, 0, 0, 1.0]
</rosparam>
</node>
</launch>


12 changes: 7 additions & 5 deletions src/ros/odometry_eval.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ OdometryEval<Stereo>::OdometryEval(const ::ros::NodeHandle &nh, const ::ros::Nod
bool logCeres;
int numCeresThreads;

this->nhp_.param("output_frame", cameraFrame_, std::string("map"));
this->nhp_.param("pnp_inlier_threshold", reprojThresh, 10.);
this->nhp_.param("pnp_iterations", iterations, 1000);
this->nhp_.param("bundle_adjustment_max_iterations", baMaxIter, 500);
Expand Down Expand Up @@ -91,7 +92,7 @@ template <bool Stereo>
void OdometryEval<Stereo>::PublishOdometry()
{
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.frame_id = "map";
poseMsg.header.frame_id = cameraFrame_;
if (this->trackingModule_->GetFrames().back()->HasEstimatedPose() || this->trackingModule_->GetFrames().size() == 1)
{
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();
Expand All @@ -107,6 +108,7 @@ void OdometryEval<Stereo>::PublishOdometry()
odometryPublisher_.publish(poseMsg);
}
const Matrix<double, 3, 4> &poseGnd = this->trackingModule_->GetFrames().back()->GetPose();
poseMsg.header.frame_id = cameraFrame_;
poseMsg.pose.position.x = poseGnd(0, 3);
poseMsg.pose.position.y = poseGnd(1, 3);
poseMsg.pose.position.z = poseGnd(2, 3);
Expand All @@ -120,18 +122,18 @@ void OdometryEval<Stereo>::PublishOdometry()

nav_msgs::Path path;
path.header.stamp = ::ros::Time::now();
path.header.frame_id = "map";
path.header.frame_id = cameraFrame_;
nav_msgs::Path pathGnd;
pathGnd.header.stamp = ::ros::Time::now();
pathGnd.header.frame_id = "map";
pathGnd.header.frame_id = cameraFrame_;
bool first = true;
for (const std::unique_ptr<data::Frame> &frame : this->trackingModule_->GetFrames())
{
if (frame->HasEstimatedPose() || first)
{
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ::ros::Time(frame->GetTime());
poseMsg.header.frame_id = "map";
poseMsg.header.frame_id = cameraFrame_;
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);
Expand All @@ -147,7 +149,7 @@ void OdometryEval<Stereo>::PublishOdometry()
{
geometry_msgs::PoseStamped poseMsg;
poseMsg.header.stamp = ::ros::Time(frame->GetTime());
poseMsg.header.frame_id = "map";
poseMsg.header.frame_id = cameraFrame_;
const Matrix<double, 3, 4> &pose = frame->GetPose();
poseMsg.pose.position.x = pose(0, 3);
poseMsg.pose.position.y = pose(1, 3);
Expand Down
3 changes: 2 additions & 1 deletion src/ros/odometry_eval.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,11 +32,12 @@ class OdometryEval : public virtual TrackingEval<Stereo>
std::unique_ptr<module::OdometryModule> odometryModule_;

private:

::ros::Publisher odometryPublisher_;
::ros::Publisher odometryGndPublisher_;
::ros::Publisher pathPublisher_;
::ros::Publisher pathGndPublisher_;

std::string cameraFrame_;
};

}
Expand Down
5 changes: 3 additions & 2 deletions src/ros/reconstruction_eval.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@ ReconstructionEval<Stereo>::ReconstructionEval(const ::ros::NodeHandle &nh, cons
bool logCeres;
int numCeresThreads;

this->nhp_.param("output_frame", cameraFrame_, std::string("map"));
this->nhp_.param("max_reprojection_error", maxReprojError, 0.0);
this->nhp_.param("min_triangulation_angle", minTriAngle, 1.0);
this->nhp_.param("bundle_adjustment_max_iterations", baMaxIter, 500);
Expand Down Expand Up @@ -68,7 +69,7 @@ void ReconstructionEval<Stereo>::Finish()
reconstructionModule_->Visualize(noarr, cloud);
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(cloud, msg);
msg.header.frame_id = "map";
msg.header.frame_id = cameraFrame_;
msg.header.stamp = ::ros::Time::now();
pointCloudPublisher_.publish(msg);
}
Expand All @@ -86,7 +87,7 @@ void ReconstructionEval<Stereo>::Visualize(cv_bridge::CvImagePtr &base_img)
reconstructionModule_->Visualize(base_img->image, cloud);
sensor_msgs::PointCloud2 msg;
pcl::toROSMsg(cloud, msg);
msg.header.frame_id = "map";
msg.header.frame_id = cameraFrame_;
msg.header.stamp = ::ros::Time::now();
pointCloudPublisher_.publish(msg);
TrackingEval<Stereo>::Visualize(base_img);
Expand Down
2 changes: 2 additions & 0 deletions src/ros/reconstruction_eval.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ class ReconstructionEval : public virtual TrackingEval<Stereo>

private:
::ros::Publisher pointCloudPublisher_;

std::string cameraFrame_;
};

}
Expand Down
6 changes: 3 additions & 3 deletions src/stereo/stereo_matcher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
if (feat != nullptr)
{
pointsToMatch.push_back(feat->GetKeypoint());
bearings1.push_back(feat->GetBearing().normalized());
bearings1.push_back(util::TFUtil::WorldFrameToCameraFrame(feat->GetBearing().normalized()));
origInx.push_back(i);
}
}
Expand All @@ -67,7 +67,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector<data::Landmark> &landma
int inx = *it;
Vector3d &bearing1 = bearings1[inx];
data::Feature feat(frame, matchedPoints[inx]);
Vector3d bearing2 = feat.GetBearing().normalized();
Vector3d bearing2 = util::TFUtil::WorldFrameToCameraFrame(feat.GetBearing().normalized());

RowVector3d epiplane1 = bearing2.transpose() * E;
epiplane1.normalize();
Expand All @@ -77,7 +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_)
{
landmarks[origInx[inx]].SetEstimatedPosition(util::TFUtil::TransformPoint(framePose, TriangulateDLT(bearing1, bearing2, I, frame.GetStereoPose())), std::vector<int>({frame.GetID()}));
landmarks[origInx[inx]].SetEstimatedPosition(util::TFUtil::TransformPoint(framePose, util::TFUtil::CameraFrameToWorldFrame(TriangulateDLT(bearing1, bearing2, I, frame.GetStereoPose()))), std::vector<int>({frame.GetID()}));
landmarks[origInx[inx]].AddStereoObservation(feat);
good++;
}
Expand Down
12 changes: 6 additions & 6 deletions src/util/tf_util.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,15 +16,15 @@ template <typename T>
inline Matrix<T, 3, 1> CameraFrameToWorldFrame(Matrix<T, 3, 1> cameraFramePt)
{
Matrix<T, 3, 1> worldFramePt;
worldFramePt << cameraFramePt(0), cameraFramePt(2), -cameraFramePt(1);
worldFramePt << cameraFramePt(2), -cameraFramePt(0), -cameraFramePt(1);
return worldFramePt;
}

template <typename T>
inline Matrix<T, 3, 1> WorldFrameToCameraFrame(Matrix<T, 3, 1> worldFramePt)
{
Matrix<T, 3, 1> cameraFramePt;
cameraFramePt << worldFramePt(0), -worldFramePt(2), worldFramePt(1);
cameraFramePt << -worldFramePt(1), -worldFramePt(2), worldFramePt(0);
return cameraFramePt;
}

Expand Down Expand Up @@ -80,20 +80,20 @@ template <typename T>
inline Matrix<T, 3, 3> GetEssentialMatrixFromPoses(Matrix<T, 3, 4> pose1, Matrix<T, 3, 4> pose2)
{
Matrix<T, 3, 4> rel = GetRelativeTransform(pose1, pose2);
Vector3d t = rel.template block<3, 1>(0, 3);
Vector3d t = -GetRotationFromPoseMatrix(rel).transpose() * rel.template block<3, 1>(0, 3);
Matrix3d tx;
tx << 0, -t(2), t(1),
t(2), 0, -t(0),
-t(1), t(0), 0;
return tx * GetRotationFromPoseMatrix(rel);
return GetRotationFromPoseMatrix(rel) * tx;
}

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();
I.template block<3, 3>(0, 0) = Matrix<T, 3, 3>::Identity();
I.template block<3, 1>(0, 3) = Matrix<T, 3, 1>::Zero();
return I;
}

Expand Down

0 comments on commit 0b2193a

Please sign in to comment.