diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch index 9d2ba00..7f90726 100644 --- a/launch/odometry_eval.launch +++ b/launch/odometry_eval.launch @@ -15,6 +15,7 @@ + camera_model: '$(arg camera_model)' diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index 686002b..890d0e1 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -12,6 +12,7 @@ + camera_model: '$(arg camera_model)' diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch index 59d8744..c1ffebd 100644 --- a/launch/slam_eval.launch +++ b/launch/slam_eval.launch @@ -18,6 +18,7 @@ + camera_model: '$(arg camera_model)' diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch new file mode 100644 index 0000000..1ba6bda --- /dev/null +++ b/launch/slam_eval_kitti.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + 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] + + + + + diff --git a/src/ros/odometry_eval.cc b/src/ros/odometry_eval.cc index ce80d71..a9b0153 100644 --- a/src/ros/odometry_eval.cc +++ b/src/ros/odometry_eval.cc @@ -25,6 +25,7 @@ OdometryEval::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); @@ -91,7 +92,7 @@ template void OdometryEval::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 &pose = (this->trackingModule_->GetFrames().size() == 1 && !this->trackingModule_->GetFrames().back()->HasEstimatedPose()) ? this->trackingModule_->GetFrames().back()->GetPose() : this->trackingModule_->GetFrames().back()->GetEstimatedPose(); @@ -107,6 +108,7 @@ void OdometryEval::PublishOdometry() odometryPublisher_.publish(poseMsg); } const Matrix &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); @@ -120,10 +122,10 @@ void OdometryEval::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 &frame : this->trackingModule_->GetFrames()) { @@ -131,7 +133,7 @@ void OdometryEval::PublishOdometry() { geometry_msgs::PoseStamped poseMsg; poseMsg.header.stamp = ::ros::Time(frame->GetTime()); - poseMsg.header.frame_id = "map"; + poseMsg.header.frame_id = cameraFrame_; const Matrix &pose = (first && !frame->HasEstimatedPose()) ? frame->GetPose() : frame->GetEstimatedPose(); poseMsg.pose.position.x = pose(0, 3); poseMsg.pose.position.y = pose(1, 3); @@ -147,7 +149,7 @@ void OdometryEval::PublishOdometry() { geometry_msgs::PoseStamped poseMsg; poseMsg.header.stamp = ::ros::Time(frame->GetTime()); - poseMsg.header.frame_id = "map"; + poseMsg.header.frame_id = cameraFrame_; const Matrix &pose = frame->GetPose(); poseMsg.pose.position.x = pose(0, 3); poseMsg.pose.position.y = pose(1, 3); diff --git a/src/ros/odometry_eval.h b/src/ros/odometry_eval.h index f654387..96a6c84 100644 --- a/src/ros/odometry_eval.h +++ b/src/ros/odometry_eval.h @@ -32,11 +32,12 @@ class OdometryEval : public virtual TrackingEval std::unique_ptr odometryModule_; private: - ::ros::Publisher odometryPublisher_; ::ros::Publisher odometryGndPublisher_; ::ros::Publisher pathPublisher_; ::ros::Publisher pathGndPublisher_; + + std::string cameraFrame_; }; } diff --git a/src/ros/reconstruction_eval.cc b/src/ros/reconstruction_eval.cc index 229f4b6..4a45e15 100644 --- a/src/ros/reconstruction_eval.cc +++ b/src/ros/reconstruction_eval.cc @@ -25,6 +25,7 @@ ReconstructionEval::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); @@ -68,7 +69,7 @@ void ReconstructionEval::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); } @@ -86,7 +87,7 @@ void ReconstructionEval::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::Visualize(base_img); diff --git a/src/ros/reconstruction_eval.h b/src/ros/reconstruction_eval.h index effc9f9..90340c9 100644 --- a/src/ros/reconstruction_eval.h +++ b/src/ros/reconstruction_eval.h @@ -31,6 +31,8 @@ class ReconstructionEval : public virtual TrackingEval private: ::ros::Publisher pointCloudPublisher_; + + std::string cameraFrame_; }; } diff --git a/src/stereo/stereo_matcher.cc b/src/stereo/stereo_matcher.cc index 39c73df..5264d2b 100644 --- a/src/stereo/stereo_matcher.cc +++ b/src/stereo/stereo_matcher.cc @@ -45,7 +45,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector &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); } } @@ -67,7 +67,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector &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(); @@ -77,7 +77,7 @@ int StereoMatcher::Match(data::Frame &frame, std::vector &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({frame.GetID()})); + landmarks[origInx[inx]].SetEstimatedPosition(util::TFUtil::TransformPoint(framePose, util::TFUtil::CameraFrameToWorldFrame(TriangulateDLT(bearing1, bearing2, I, frame.GetStereoPose()))), std::vector({frame.GetID()})); landmarks[origInx[inx]].AddStereoObservation(feat); good++; } diff --git a/src/util/tf_util.h b/src/util/tf_util.h index 6a45c3e..449f228 100644 --- a/src/util/tf_util.h +++ b/src/util/tf_util.h @@ -16,7 +16,7 @@ template inline Matrix CameraFrameToWorldFrame(Matrix cameraFramePt) { Matrix worldFramePt; - worldFramePt << cameraFramePt(0), cameraFramePt(2), -cameraFramePt(1); + worldFramePt << cameraFramePt(2), -cameraFramePt(0), -cameraFramePt(1); return worldFramePt; } @@ -24,7 +24,7 @@ template inline Matrix WorldFrameToCameraFrame(Matrix worldFramePt) { Matrix cameraFramePt; - cameraFramePt << worldFramePt(0), -worldFramePt(2), worldFramePt(1); + cameraFramePt << -worldFramePt(1), -worldFramePt(2), worldFramePt(0); return cameraFramePt; } @@ -80,20 +80,20 @@ template inline Matrix GetEssentialMatrixFromPoses(Matrix pose1, Matrix pose2) { Matrix 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 inline Matrix IdentityPoseMatrix() { Matrix 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::Identity(); + I.template block<3, 1>(0, 3) = Matrix::Zero(); return I; }