Skip to content

GitLab

Commit f1f45862 authored by dla.adolfsson@gmail.com's avatar dla.adolfsson@gmail.com
Browse files

changed to ros::Tim instead of double, going from double to ros::Time t... 

changed to ros::Tim instead of double, going from double to ros::Time t crashes the program, perhaps because time is initialized to -1 at some point?
parent 8a554737
Hide whitespace changes
Inline Side-by-side
Showing with 41 additions and 30 deletions
ndt_calibration/include/ndt_calibration/ndt_calib.h
View file @ f1f45862
......@@ -35,7 +35,7 @@ public:
bool valid_pose; // if the estimated pose is valid (in case the inpolation is not possible this is false)
Eigen::Affine3d originalPose; // estimated pose in global frame (preferably from a GT system)...
Eigen::Affine3d estSensorPose; // estimated sensor pose in global frame (typically from a SLAM / registration system)
double stamp; // timestamp of originalPose
ros::Time stamp; // timestamp of originalPose
boost::shared_ptr<perception_oru::NDTMap> ndtmap;
boost::shared_ptr<perception_oru::LazyGrid> lazygrid;
......@@ -44,15 +44,15 @@ public:
}
NDTCalibScan(const Eigen::Affine3d &_pose, const Eigen::Affine3d &_estSensorPose, double _stamp) : pose(_pose), originalPose(_pose), estSensorPose(_estSensorPose), stamp(_stamp), ndtmap(NULL), valid_pose(false) {
NDTCalibScan(const Eigen::Affine3d &_pose, const Eigen::Affine3d &_estSensorPose, const ros::Time &_stamp) : pose(_pose), originalPose(_pose), estSensorPose(_estSensorPose), stamp(_stamp), ndtmap(NULL), valid_pose(false) {
}
NDTCalibScan(const pcl::PointCloud<pcl::PointXYZ> &_cloud, const Eigen::Affine3d &_pose, double _stamp) : cloud(_cloud), pose(_pose), originalPose(_pose), stamp(_stamp), ndtmap(NULL), valid_pose(false) {
NDTCalibScan(const pcl::PointCloud<pcl::PointXYZ> &_cloud, const Eigen::Affine3d &_pose, const ros::Time &_stamp) : cloud(_cloud), pose(_pose), originalPose(_pose), stamp(_stamp), ndtmap(NULL), valid_pose(false) {
}
NDTCalibScan(const pcl::PointCloud<pcl::PointXYZ> &_cloud, const Eigen::Affine3d &_pose, const Eigen::Affine3d &_estSensorPose, double _stamp) : cloud(_cloud), pose(_pose), originalPose(_pose), estSensorPose(_estSensorPose), stamp(_stamp), ndtmap(NULL), valid_pose(false)
NDTCalibScan(const pcl::PointCloud<pcl::PointXYZ> &_cloud, const Eigen::Affine3d &_pose, const Eigen::Affine3d &_estSensorPose, const ros::Time &_stamp) : cloud(_cloud), pose(_pose), originalPose(_pose), estSensorPose(_estSensorPose), stamp(_stamp), ndtmap(NULL), valid_pose(false)
{
}
......@@ -113,7 +113,7 @@ public:
}
void printDebug() const {
std::cout << " stamp : " << this->stamp << std::flush;
std::cout << " stamp : "<<this->stamp << std::flush;
}
};
......@@ -129,7 +129,7 @@ public:
}
NDTCalibScanPair(const pcl::PointCloud<pcl::PointXYZ> &p1, const Eigen::Affine3d &pose1,
const pcl::PointCloud<pcl::PointXYZ> &p2, const Eigen::Affine3d &pose2) :
first(NDTCalibScan(p1, pose1, -1.)), second(NDTCalibScan(p2, pose2, -1.))
first(NDTCalibScan(p1, pose1, /*daniel changed from (double) -1*/ros::Time(0) )), second(NDTCalibScan(p2, pose2, /*daniel changed from (double) -1*/ros::Time(0)))
{
}
......@@ -310,12 +310,12 @@ public:
}
// Return time stamp, the timeOffset is simply added to the original pose.
std::vector<double> getTimeStamps(double timeOffset, bool oneInEachPair = true) const {
std::vector<double> ret;
std::vector<ros::Time> getTimeStamps(double timeOffset, bool oneInEachPair = true) const {
std::vector<ros::Time> ret;
for (size_t i = 0; i < this->size(); i++) {
ret.push_back((*this)[i].first.stamp + timeOffset);
ret.push_back((*this)[i].first.stamp + ros::Duration(timeOffset) );
if (!oneInEachPair) {
ret.push_back((*this)[i].second.stamp + timeOffset);
ret.push_back((*this)[i].second.stamp + ros::Duration(timeOffset) );
}
}
return ret;
......@@ -465,7 +465,7 @@ private:
void assignParameters(const Eigen::VectorXd &params, Eigen::Affine3d &Ts, double &t);
bool interpPose(double time, Eigen::Affine3d &T, bool printout = true);
bool interpPose(const ros::Time &time, Eigen::Affine3d &T, bool printout = true);
double getScoreTime(double sensorTimeOffset);
PoseInterpolationInteface/*PoseInterpolationNavMsgsOdo*/ &_poseInterp;
......@@ -485,11 +485,15 @@ void loadNDTCalibScanPairs(const std::string &gt_file, const std::string &est_se
scans.resize(0);
// Load the data...
std::vector<double> stamps = ndt_generic::loadTimeStampFromEvalFile(gt_file);
std::vector<double> stamps_double = ndt_generic::loadTimeStampFromEvalFile(gt_file);
std::vector<ros::Time> stamps_ros;
for(int i=0;i<stamps_double.size();i++)
stamps_ros.push_back( ros::Time(stamps_double[i]) );
std::vector<Eigen::Affine3d> Tgt = ndt_generic::loadAffineFromEvalFile(gt_file);
std::vector<Eigen::Affine3d> Test_sensorpose = ndt_generic::loadAffineFromEvalFile(est_sensorpose_file);
if (Tgt.size() != stamps.size() || Tgt.size() != Test_sensorpose.size()) {
if (Tgt.size() != stamps_ros.size() || Tgt.size() != Test_sensorpose.size()) {
std::cerr << "Warning: the length of the provided files to not match(!)" << std::endl;
}
......@@ -514,8 +518,8 @@ void loadNDTCalibScanPairs(const std::string &gt_file, const std::string &est_se
// Good pair found...
NDTCalibScanPair pair;
pair.first = NDTCalibScan(Tgt[j], Test_sensorpose[j], stamps[j]);
pair.second = NDTCalibScan(Tgt[i], Test_sensorpose[i], stamps[i]);
pair.first = NDTCalibScan(Tgt[j], Test_sensorpose[j], stamps_ros[j]);
pair.second = NDTCalibScan(Tgt[i], Test_sensorpose[i], stamps_ros[i]);
std::cout << "#scans : " << scans.size() << " [max : " << max_nb_pairs << "]" << std::endl;
if (max_nb_pairs > 0 && scans.size() >= max_nb_pairs) {
......
ndt_calibration/src/ndt_calib.cpp
View file @ f1f45862
......@@ -160,7 +160,7 @@ double NDTCalibOptimize::getScore(const Eigen::VectorXd &x) {
return getScore6d(Ts);
}
bool NDTCalibOptimize::interpPose(double time, Eigen::Affine3d &T, bool printout) {
bool NDTCalibOptimize::interpPose(const ros::Time &time, Eigen::Affine3d &T, bool printout) {
std::cout<<"double time:"<<time<<std::endl;
ros::Time t(time);
std::cout<<"Ros Time for pair:"<<t<<std::endl;
......@@ -174,8 +174,8 @@ bool NDTCalibOptimize::interpPose(double time, Eigen::Affine3d &T, bool printout
void NDTCalibOptimize::interpPairPoses(double sensorTimeOffset) {
for (int i = 0; i < _pairs.size(); i++) {
_pairs[i].first.valid_pose = interpPose(_pairs[i].first.stamp - sensorTimeOffset, _pairs[i].first.pose, _pairs[i].first.valid_pose);
_pairs[i].second.valid_pose = interpPose(_pairs[i].second.stamp - sensorTimeOffset, _pairs[i].second.pose, _pairs[i].first.valid_pose);
_pairs[i].first.valid_pose = interpPose(_pairs[i].first.stamp - ros::Duration(sensorTimeOffset) , _pairs[i].first.pose, _pairs[i].first.valid_pose);
_pairs[i].second.valid_pose = interpPose(_pairs[i].second.stamp - ros::Duration(sensorTimeOffset) , _pairs[i].second.pose, _pairs[i].first.valid_pose);
}
}
......
ndt_calibration/src/ndt_calib_online_node.cpp
View file @ f1f45862
......@@ -33,7 +33,7 @@
#include "velodyne_msgs/VelodyneScan.h"
#include <velodyne_pointcloud/rawdata.h>
#include <velodyne_pointcloud/point_types.h>
#include "pcl_conversions/pcl_conversions.h"
//! Simple interface class implementation to be compatible with the NDTCalib API.
class PoseInterpolationTF : public PoseInterpolationInteface {
......@@ -44,7 +44,6 @@ public:
bool getTransformationForTime(ros::Time t0, const std::string &frame_id, Eigen::Affine3d &T){
std::string str;
std::cout<<"Get transformation from"<<fixedframe_<<" to "<<frame_id<<std::endl;
if (!transformer_.canTransform(fixedframe_, frame_id, t0, &str)) {
return false;
}
......@@ -214,29 +213,38 @@ public:
// The T is given in global vehicle coordinates (not any relative ones as commonly done in the registration / fuser approaches.
// Cloud should be given in the sensor frame.
bool processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, Eigen::Affine3d T, ros::Time stamp) {
bool processFrame(pcl::PointCloud<pcl::PointXYZ> &cloud, Eigen::Affine3d &T, const ros::Time &stamp) {
//boost::mutex::scoped_lock lock(pairs_mutex_);
if (!initialized) {
scan_pair_.first = NDTCalibScan(cloud, T, cloud.header.stamp);
ros::Time t_cloud;
pcl_conversions::fromPCL(cloud.header.stamp,t_cloud);
scan_pair_.first = NDTCalibScan(cloud, T, t_cloud);
initialized = true;
return true;
}
Eigen::Affine3d Tmotion = scan_pair_.first.getOriginalPose().inverse()*T;
// We have gone to far without any "rotation", simply reset the system.
if (Tmotion.translation().norm() > max_translation_) {
scan_pair_.first = NDTCalibScan(cloud, T, cloud.header.stamp);
ros::Time t_cloud;
pcl_conversions::fromPCL(cloud.header.stamp,t_cloud);
scan_pair_.first = NDTCalibScan(cloud, T, t_cloud);
}
Eigen::Vector3d rot = Tmotion.rotation().eulerAngles(0,1,2);
normalizeEulerAngles(rot);
if (rot.norm() < min_rotation_) {
return false;
}
ros::Time tcloud;
// Good pair found...
scan_pair_.second = NDTCalibScan(cloud, T, stamp.toSec());
scan_pair_.second = NDTCalibScan(cloud, T, stamp);
scan_pair_.printDebug();
pairs_mutex_.lock();
pairs_.push_back(scan_pair_);
......@@ -307,9 +315,8 @@ public:
Eigen::Affine3d T;
ros::Time t = msg->header.stamp-ros::Duration(sensor_offset_t_);
if (use_scan && pose_interp_->getTransformationForTime(t, pose_frame_, T)) {
if (this->processFrame(cloud,T, msg->header.stamp-ros::Duration(sensor_offset_t_))) {
if (this->processFrame(cloud, T, msg->header.stamp-ros::Duration(sensor_offset_t_))) {
// Great this point was used.
counter_ = 0;
}
......
ndt_calibration/src/ndt_calib_test.cpp
View file @ f1f45862
......@@ -43,7 +43,7 @@ int main(int argc, char **argv){
{
Eigen::Affine3d T1 = getAsAffine3d(Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0));
NDTCalibScan scan(T1, T1, 0.);
NDTCalibScan scan(T1, T1, ros::Time(0));
std::cout << "Should give zeros";
std::cout << "scan.getTs() : " << affine3dToString(scan.getTs()) << std::endl;
}
......@@ -51,7 +51,7 @@ int main(int argc, char **argv){
{
Eigen::Affine3d T1 = getAsAffine3d(Eigen::Vector3d(1, 0, 0), Eigen::Vector3d(0, 0, 0));
Eigen::Affine3d T2 = getAsAffine3d(Eigen::Vector3d(2, 0, 0), Eigen::Vector3d(0, 0, M_PI/2.));
NDTCalibScan scan(T1, T2, 0.);
NDTCalibScan scan(T1, T2, ros::Time(0));
std::cout << "Should give 1 0 0 0 0 M_PI/2..." << std::endl;
std::cout << "scan.getTs() : " << affine3dToString(scan.getTs()) << std::endl;
}
......@@ -61,8 +61,8 @@ int main(int argc, char **argv){
Eigen::Affine3d T2 = getAsAffine3d(Eigen::Vector3d(2, 0, 0), Eigen::Vector3d(0, 0, M_PI/2.));
Eigen::Affine3d Ts = getAsAffine3d(Eigen::Vector3d(3.,0, 1.2), Eigen::Vector3d(0,0,0));
NDTCalibScan scan1(T1, T1*Ts, 0.);
NDTCalibScan scan2(T2, T2*Ts, 0.);
NDTCalibScan scan1(T1, T1*Ts, ros::Time(0));
NDTCalibScan scan2(T2, T2*Ts, ros::Time(0));
NDTCalibScanPair pair;
pair.first = scan1;
pair.second = scan2;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment