diff --git a/src/capture_realsense_video.cpp b/src/capture_realsense_video.cpp index 721e249..dd9f884 100644 --- a/src/capture_realsense_video.cpp +++ b/src/capture_realsense_video.cpp @@ -86,16 +86,17 @@ class CaptureVideo : public rclcpp::Node tf2::Quaternion q; q.setRPY(roll, pitch, yaw); - imu_file << msg.linear_acceleration.x << ", " << msg.linear_acceleration.y << + timestamp = msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9; + imu_file << std::fixed << std::setprecision(9) << msg.linear_acceleration.x << ", " << msg.linear_acceleration.y << ", " << msg.linear_acceleration.z << ", " << msg.angular_velocity.x << ", " << - msg.angular_velocity.y << ", " << msg.angular_velocity.z << ", " << msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9 << std::endl; + msg.angular_velocity.y << ", " << msg.angular_velocity.z << ", " << timestamp << std::endl; } void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { auto cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::MONO8); output_video << cv_ptr->image; - image_timestamp_file << std::to_string(msg->header.stamp.sec + msg->header.stamp.nanosec * 1e-9) << std::endl; + image_timestamp_file << std::fixed << std::setprecision(9) << timestamp << std::endl; cv::imshow("image", cv_ptr->image); cv::waitKey(1); @@ -107,6 +108,7 @@ class CaptureVideo : public rclcpp::Node std::ofstream imu_file; std::ofstream image_timestamp_file; std::string time_string; + double timestamp; }; int main(int argc, char * argv[]) diff --git a/src/imu_mono_realsense.cpp b/src/imu_mono_realsense.cpp index 76ca5ae..564da95 100644 --- a/src/imu_mono_realsense.cpp +++ b/src/imu_mono_realsense.cpp @@ -40,7 +40,8 @@ class ImuMonoRealSense : public rclcpp::Node ImuMonoRealSense() : Node("imu_mono_realsense"), vocabulary_file_path(std::string(PROJECT_PATH)+ "/orb_slam3/Vocabulary/ORBvoc.txt.bin"), - count(0) + count(0), + count2(0) { // declare parameters @@ -134,7 +135,7 @@ class ImuMonoRealSense : public rclcpp::Node // create publishers // create timer - image_timer = create_wall_timer(1s/30, std::bind(&ImuMonoRealSense::image_timer_callback, this)); + image_timer = create_wall_timer(1s/30, std::bind(&ImuMonoRealSense::image_timer_callback, this), image_timer_callback_group); imu_timer = create_wall_timer(1s/200, std::bind(&ImuMonoRealSense::imu_timer_callback, this)); } @@ -219,11 +220,12 @@ class ImuMonoRealSense : public rclcpp::Node msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z, msg.header.stamp.sec + msg.header.stamp.nanosec * 1e-9); vImuMeas.push_back(imu_meas); + + } void image_timer_callback() { - RCLCPP_INFO(get_logger(), "image timer callback"); if (!use_live_feed) { cv::Mat frame; input_video >> frame; @@ -238,6 +240,9 @@ class ImuMonoRealSense : public rclcpp::Node timestamp_data.push_back(std::stof(token)); } + timestamp = timestamp_data[0] + timestamp_data[1] * 1e-9; + RCLCPP_INFO_STREAM(get_logger(), "image timestamp: " << timestamp); + if (!frame.empty()) { if(image_scale != 1.f) { cv::resize(frame, frame, cv::Size(frame.cols*image_scale, frame.rows*image_scale)); @@ -274,13 +279,52 @@ class ImuMonoRealSense : public rclcpp::Node std::getline(imu_file, imu_line); std::istringstream ss(imu_line); std::string token; - std::vector imu_data; + std::vector imu_data; while(std::getline(ss, token, ',')) { - imu_data.push_back(std::stof(token)); + imu_data.push_back(std::stod(token)); } ORB_SLAM3::IMU::Point imu_meas(imu_data[0], imu_data[1], imu_data[2], imu_data[3], imu_data[4], imu_data[5], imu_data[6]); + RCLCPP_INFO_STREAM(get_logger(), "imu timestamp: " << std::fixed << std::setprecision(9) << imu_data[6] << std::endl); vImuMeas.push_back(imu_meas); + + + if (count2 >= 200/30) { + count2 = 0; + if (!use_live_feed) { + cv::Mat frame; + input_video >> frame; + auto ros_image = cv_bridge::CvImage(std_msgs::msg::Header(), "mono8", frame).toImageMsg(); + + std::string timestamp_line; + std::getline(video_timestamp_file, timestamp_line); + timestamp = std::stod(timestamp_line); + RCLCPP_INFO_STREAM(get_logger(), "image timestamp: " << std::fixed << std::setprecision(9)<< timestamp); + + if (!frame.empty()) { + if(image_scale != 1.f) { + cv::resize(frame, frame, cv::Size(frame.cols*image_scale, frame.rows*image_scale)); + } + + if (sensor_type_param == "monocular") { + Sophus::SE3f Tcw = pAgent->TrackMonocular(frame, timestamp); + } else if (sensor_type_param == "imu-monocular"){ + Sophus::SE3f Tcw = pAgent->TrackMonocular(frame, timestamp, vImuMeas); + } + count ++; + + cv::imshow("frame", frame); + cv::waitKey(1); + } else { + RCLCPP_INFO_STREAM_ONCE(get_logger(), "End of video file"); + vectormap_points = pAgent->GetTrackedMapPoints(); + save_map_to_csv(map_points); + pAgent->Shutdown(); + rclcpp::shutdown(); + } + } + } + count2++; } } @@ -314,6 +358,7 @@ class ImuMonoRealSense : public rclcpp::Node std::shared_ptr initial_orientation; int count; + int count2; }; int main(int argc, char * argv[]) diff --git a/src/visualize_point_cloud.cpp b/src/visualize_point_cloud.cpp index dc386c7..1656c5d 100644 --- a/src/visualize_point_cloud.cpp +++ b/src/visualize_point_cloud.cpp @@ -182,15 +182,15 @@ class VisualizePointCloud : public rclcpp::Node } // now i can filter the pointcloud - // pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - // pcl::fromROSMsg(point_cloud2, *cloud); - // pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); - // pcl::StatisticalOutlierRemoval sor; - // sor.setInputCloud(cloud); - // sor.setMeanK(50); - // sor.setStddevMulThresh(1.0); - // sor.filter(*cloud_filtered); - // pcl::toROSMsg(*cloud_filtered, point_cloud2); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromROSMsg(point_cloud2, *cloud); + pcl::PointCloud::Ptr cloud_filtered(new pcl::PointCloud); + pcl::StatisticalOutlierRemoval sor; + sor.setInputCloud(cloud); + sor.setMeanK(75); + sor.setStddevMulThresh(1.0); + sor.filter(*cloud_filtered); + pcl::toROSMsg(*cloud_filtered, point_cloud2); } void timer_callback()