Skip to content

Commit

Permalink
20.04 opencv 4.0 compatability
Browse files Browse the repository at this point in the history
  • Loading branch information
@harveybia
harveybia committed Sep 3, 2021
1 parent 46d3d93 commit d54d97e
Show file tree
Hide file tree
Showing 5 changed files with 29 additions and 29 deletions.
10 changes: 5 additions & 5 deletions SC-PGO/src/kittiHelper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,12 +58,12 @@ int main(int argc, char** argv)

ros::Publisher pubOdomGT = n.advertise<nav_msgs::Odometry> ("/odometry_gt", 5);
nav_msgs::Odometry odomGT;
odomGT.header.frame_id = "/camera_init";
odomGT.header.frame_id = "camera_init";
odomGT.child_frame_id = "/ground_truth";

ros::Publisher pubPathGT = n.advertise<nav_msgs::Path> ("/path_gt", 5);
nav_msgs::Path pathGT;
pathGT.header.frame_id = "/camera_init";
pathGT.header.frame_id = "camera_init";

std::string timestamp_path = "sequences/" + sequence_number + "/times.txt";
std::ifstream timestamp_file(dataset_folder + timestamp_path, std::ifstream::in);
Expand All @@ -88,9 +88,9 @@ int main(int argc, char** argv)
float timestamp = stof(line);
std::stringstream left_image_path, right_image_path;
left_image_path << dataset_folder << "sequences/" + sequence_number + "/image_0/" << std::setfill('0') << std::setw(6) << line_num << ".png";
cv::Mat left_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
cv::Mat left_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);
right_image_path << dataset_folder << "sequences/" + sequence_number + "/image_1/" << std::setfill('0') << std::setw(6) << line_num << ".png";
cv::Mat right_image = cv::imread(left_image_path.str(), CV_LOAD_IMAGE_GRAYSCALE);
cv::Mat right_image = cv::imread(left_image_path.str(), cv::IMREAD_GRAYSCALE);

std::getline(ground_truth_file, line);
std::stringstream pose_stream(line);
Expand Down Expand Up @@ -153,7 +153,7 @@ int main(int argc, char** argv)
sensor_msgs::PointCloud2 laser_cloud_msg;
pcl::toROSMsg(laser_cloud, laser_cloud_msg);
laser_cloud_msg.header.stamp = ros::Time().fromSec(timestamp);
laser_cloud_msg.header.frame_id = "/camera_init";
laser_cloud_msg.header.frame_id = "camera_init";
pub_laser_cloud.publish(laser_cloud_msg);

sensor_msgs::ImagePtr image_left_msg = cv_bridge::CvImage(laser_cloud_msg.header, "mono8", left_image).toImageMsg();
Expand Down
16 changes: 8 additions & 8 deletions SC-PGO/src/laserMapping.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -216,7 +216,7 @@ void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;

nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = laserOdometry->header.stamp;
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
Expand Down Expand Up @@ -817,7 +817,7 @@ void process()
sensor_msgs::PointCloud2 laserCloudSurround3;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround3);
laserCloudSurround3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudSurround3.header.frame_id = "/camera_init";
laserCloudSurround3.header.frame_id = "camera_init";
pubLaserCloudSurround.publish(laserCloudSurround3);
}

Expand All @@ -832,14 +832,14 @@ void process()
sensor_msgs::PointCloud2 laserCloudMsg;
pcl::toROSMsg(laserCloudMap, laserCloudMsg);
laserCloudMsg.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudMsg.header.frame_id = "/camera_init";
laserCloudMsg.header.frame_id = "camera_init";
pubLaserCloudMap.publish(laserCloudMsg);
}

sensor_msgs::PointCloud2 laserCloudFullRes3Local;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3Local);
laserCloudFullRes3Local.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3Local.header.frame_id = "/camera_init";
laserCloudFullRes3Local.header.frame_id = "camera_init";
pubLaserCloudFullResLocal.publish(laserCloudFullRes3Local);

int laserCloudFullResNum = laserCloudFullRes->points.size();
Expand All @@ -851,15 +851,15 @@ void process()
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeLaserOdometry);
laserCloudFullRes3.header.frame_id = "/camera_init";
laserCloudFullRes3.header.frame_id = "camera_init";
pubLaserCloudFullRes.publish(laserCloudFullRes3);

//printf("mapping pub time %f ms \n", t_pub.toc());

//printf("whole mapping time %f ms ++++++++++\n", t_whole.toc());

nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init";
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "/aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserOdometry);
odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
Expand All @@ -881,7 +881,7 @@ void process()
laserAfterMappedPose.pose = odomAftMapped.pose.pose;

laserAfterMappedPath.header.stamp = odomAftMapped.header.stamp;
laserAfterMappedPath.header.frame_id = "/camera_init";
laserAfterMappedPath.header.frame_id = "camera_init";
laserAfterMappedPath.poses.push_back(laserAfterMappedPose);
pubLaserAfterMappedPath.publish(laserAfterMappedPath);

Expand All @@ -896,7 +896,7 @@ void process()
q.setY(q_w_curr.y());
q.setZ(q_w_curr.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "/camera_init", "/aft_mapped"));
br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "/aft_mapped"));

frameCount++;
}
Expand Down
4 changes: 2 additions & 2 deletions SC-PGO/src/laserOdometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -509,7 +509,7 @@ int main(int argc, char **argv)

// publish odometry
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.header.frame_id = "camera_init";
laserOdometry.child_frame_id = "/laser_odom";
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometry.pose.pose.orientation.x = q_w_curr.x();
Expand All @@ -526,7 +526,7 @@ int main(int argc, char **argv)
laserPose.pose = laserOdometry.pose.pose;
laserPath.header.stamp = laserOdometry.header.stamp;
laserPath.poses.push_back(laserPose);
laserPath.header.frame_id = "/camera_init";
laserPath.header.frame_id = "camera_init";
pubLaserPath.publish(laserPath);

// transform corner features and plane features to the scan end point
Expand Down
14 changes: 7 additions & 7 deletions SC-PGO/src/laserPosegraphOptimization.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -293,7 +293,7 @@ void pubPath( void )
// pub odom and path
nav_msgs::Odometry odomAftPGO;
nav_msgs::Path pathAftPGO;
pathAftPGO.header.frame_id = "/camera_init";
pathAftPGO.header.frame_id = "camera_init";
mKF.lock();
// for (int node_idx=0; node_idx < int(keyframePosesUpdated.size()) - 1; node_idx++) // -1 is just delayed visualization (because sometimes mutexed while adding(push_back) a new one)
for (int node_idx=0; node_idx < recentIdxUpdated; node_idx++) // -1 is just delayed visualization (because sometimes mutexed while adding(push_back) a new one)
Expand All @@ -302,7 +302,7 @@ void pubPath( void )
// const gtsam::Pose3& pose_est = isamCurrentEstimate.at<gtsam::Pose3>(node_idx);

nav_msgs::Odometry odomAftPGOthis;
odomAftPGOthis.header.frame_id = "/camera_init";
odomAftPGOthis.header.frame_id = "camera_init";
odomAftPGOthis.child_frame_id = "/aft_pgo";
odomAftPGOthis.header.stamp = ros::Time().fromSec(keyframeTimes.at(node_idx));
odomAftPGOthis.pose.pose.position.x = pose_est.x;
Expand All @@ -316,7 +316,7 @@ void pubPath( void )
poseStampAftPGO.pose = odomAftPGOthis.pose.pose;

pathAftPGO.header.stamp = odomAftPGOthis.header.stamp;
pathAftPGO.header.frame_id = "/camera_init";
pathAftPGO.header.frame_id = "camera_init";
pathAftPGO.poses.push_back(poseStampAftPGO);
}
mKF.unlock();
Expand All @@ -332,7 +332,7 @@ void pubPath( void )
q.setY(odomAftPGO.pose.pose.orientation.y);
q.setZ(odomAftPGO.pose.pose.orientation.z);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, odomAftPGO.header.stamp, "/camera_init", "/aft_pgo"));
br.sendTransform(tf::StampedTransform(transform, odomAftPGO.header.stamp, "camera_init", "/aft_pgo"));
} // pubPath

void updatePoses(void)
Expand Down Expand Up @@ -436,12 +436,12 @@ std::optional<gtsam::Pose3> doICPVirtualRelative( int _loop_kf_idx, int _curr_kf
// loop verification
sensor_msgs::PointCloud2 cureKeyframeCloudMsg;
pcl::toROSMsg(*cureKeyframeCloud, cureKeyframeCloudMsg);
cureKeyframeCloudMsg.header.frame_id = "/camera_init";
cureKeyframeCloudMsg.header.frame_id = "camera_init";
pubLoopScanLocal.publish(cureKeyframeCloudMsg);

sensor_msgs::PointCloud2 targetKeyframeCloudMsg;
pcl::toROSMsg(*targetKeyframeCloud, targetKeyframeCloudMsg);
targetKeyframeCloudMsg.header.frame_id = "/camera_init";
targetKeyframeCloudMsg.header.frame_id = "camera_init";
pubLoopSubmapLocal.publish(targetKeyframeCloudMsg);

// ICP Settings
Expand Down Expand Up @@ -750,7 +750,7 @@ void pubMap(void)

sensor_msgs::PointCloud2 laserCloudMapPGOMsg;
pcl::toROSMsg(*laserCloudMapPGO, laserCloudMapPGOMsg);
laserCloudMapPGOMsg.header.frame_id = "/camera_init";
laserCloudMapPGOMsg.header.frame_id = "camera_init";
pubMapAftPGO.publish(laserCloudMapPGOMsg);
}

Expand Down
14 changes: 7 additions & 7 deletions SC-PGO/src/scanRegistration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include <nav_msgs/Odometry.h>
#include <opencv/cv.h>
#include <opencv2/opencv.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
Expand Down Expand Up @@ -426,31 +426,31 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
laserCloudOutMsg.header.frame_id = "camera_init";
pubLaserCloud.publish(laserCloudOutMsg);

sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
cornerPointsSharpMsg.header.frame_id = "camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);

sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
cornerPointsLessSharpMsg.header.frame_id = "camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
surfPointsFlat2.header.frame_id = "camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);

sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
surfPointsLessFlat2.header.frame_id = "camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
// std::cout << "published ... " << std::endl;

Expand All @@ -462,7 +462,7 @@ void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
sensor_msgs::PointCloud2 scanMsg;
pcl::toROSMsg(laserCloudScans[i], scanMsg);
scanMsg.header.stamp = laserCloudMsg->header.stamp;
scanMsg.header.frame_id = "/camera_init";
scanMsg.header.frame_id = "camera_init";
pubEachScan[i].publish(scanMsg);
}
}
Expand Down

0 comments on commit d54d97e

Please sign in to comment.