changed timestamp to be ROS timestamp.

xiaokeme · Sep 29, 2014 · f363ee7 · f363ee7
1 parent b948119
commit f363ee7
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 2 deletions.
diff --git a/lsd_slam_core/CMakeLists.txt b/lsd_slam_core/CMakeLists.txt
@@ -38,7 +38,7 @@ add_definitions("-DENABLE_SSE")
 
 # Also add some useful compiler flag
 set(CMAKE_CXX_FLAGS
-   "${CMAKE_CXX_FLAGS} ${SSE_FLAGS}  -std=c++0x -g"
+   "${CMAKE_CXX_FLAGS} ${SSE_FLAGS} -march=native  -std=c++0x"
 ) 
 
 # Set source files

diff --git a/lsd_slam_core/src/IOWrapper/ROS/ROSImageStreamThread.cpp b/lsd_slam_core/src/IOWrapper/ROS/ROSImageStreamThread.cpp
@@ -127,7 +127,10 @@ void ROSImageStreamThread::vidCb(const sensor_msgs::ImageConstPtr img)
 	lastSEQ = img->header.seq;
 
 	TimestampedMat bufferItem;
-	bufferItem.timestamp = Timestamp::now(); // TODO: should use time of img object, but should be consistent with other timestamps
+	if(img->header.stamp.toSec() != 0)
+		bufferItem.timestamp =  Timestamp(img->header.stamp.toSec());
+	else
+		bufferItem.timestamp =  Timestamp(ros::Time::now().toSec());
 
 	if(undistorter != 0)
 	{

diff --git a/lsd_slam_core/src/IOWrapper/Timestamp.cpp b/lsd_slam_core/src/IOWrapper/Timestamp.cpp
@@ -34,10 +34,17 @@ boost::mutex Timestamp::localtimeMutex;
 
 Timestamp::Timestamp()
 {
+	externalStamp = 0;
+}
+
+Timestamp::Timestamp(double seconds)
+{
+	externalStamp = seconds;
 }
 
 double Timestamp::toSec() const
 {
+	if(externalStamp!=0) return externalStamp;
 	return std::chrono::duration<double>(timePoint - startupTimePoint).count();
 }
 

diff --git a/lsd_slam_core/src/IOWrapper/Timestamp.h b/lsd_slam_core/src/IOWrapper/Timestamp.h
@@ -51,6 +51,7 @@ class Timestamp
 	 * Creates an uninitialized timestamp.
 	 */
 	Timestamp();
+	Timestamp(double seconds);
 
 	/**
 	 * Returns the timestamp as the time in seconds which has passed since the
@@ -79,6 +80,8 @@ class Timestamp
 
 	static const std::chrono::monotonic_clock::time_point startupTimePoint;
 	static boost::mutex localtimeMutex;
+
+	double externalStamp;
 };
 }
 #endif