Proper way of exiting when using ROS

CMakeLists.txt

@@ -22,6 +22,8 @@ find_package(LibZip QUIET)
 find_package(Pangolin 0.2 QUIET)
 find_package(OpenCV QUIET)
 
+#checking for ROS
+find_package(roscpp QUIET)
 
 
 # flags
@@ -65,6 +67,16 @@ include_directories(
   ${EIGEN3_INCLUDE_DIR}
 ) 
 
+# decide if we have ROS
+if(NOT roscpp_FOUND)
+  set(roscpp_LIBRARIES "")
+  message(STATUS "Couldn't find roscpp.")
+else()
+  message(STATUS "Found roscpp installed at ${roscpp_DIR}.")
+  include_directories(${roscpp_INCLUDE_DIRS})
+  add_definitions(-DHAS_ROS=1)
+
+endif()
 
 # decide if we have pangolin
 if (Pangolin_FOUND)
@@ -120,7 +132,7 @@ add_library(dso ${dso_SOURCE_FILES} ${dso_opencv_SOURCE_FILES} ${dso_pangolin_SO
 if (OpenCV_FOUND AND Pangolin_FOUND)
 	message("--- compiling dso_dataset.")
 	add_executable(dso_dataset ${PROJECT_SOURCE_DIR}/src/main_dso_pangolin.cpp )
-    target_link_libraries(dso_dataset dso boost_system boost_thread cxsparse ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBS})
+    target_link_libraries(dso_dataset dso boost_system boost_thread cxsparse ${LIBZIP_LIBRARY} ${Pangolin_LIBRARIES} ${OpenCV_LIBS} ${roscpp_LIBRARIES})
 else()
 	message("--- not building dso_dataset, since either don't have openCV or Pangolin.")
 endif()

src/FullSystem/CoarseInitializer.cpp

@@ -325,7 +325,7 @@ void CoarseInitializer::debugPlot(int lvl, std::vector<IOWrap::Output3DWrapper*>
 Vec3f CoarseInitializer::calcResAndGS(
 		int lvl, Mat88f &H_out, Vec8f &b_out,
 		Mat88f &H_out_sc, Vec8f &b_out_sc,
-		const SE3 &refToNew, AffLight refToNew_aff,
+		SE3 refToNew, AffLight refToNew_aff,
 		bool plot)
 {
 	int wl = w[lvl], hl = h[lvl];
@@ -363,16 +363,16 @@ Vec3f CoarseInitializer::calcResAndGS(
 			continue;
 		}
 
-        VecNRf dp0;
-        VecNRf dp1;
-        VecNRf dp2;
-        VecNRf dp3;
-        VecNRf dp4;
-        VecNRf dp5;
-        VecNRf dp6;
-        VecNRf dp7;
-        VecNRf dd;
-        VecNRf r;
+        EIGEN_ALIGN32 VecNRf dp0;
+        EIGEN_ALIGN32 VecNRf dp1;
+        EIGEN_ALIGN32 VecNRf dp2;
+        EIGEN_ALIGN32 VecNRf dp3;
+        EIGEN_ALIGN32 VecNRf dp4;
+        EIGEN_ALIGN32 VecNRf dp5;
+        EIGEN_ALIGN32 VecNRf dp6;
+        EIGEN_ALIGN32 VecNRf dp7;
+        EIGEN_ALIGN32 VecNRf dd;
+        EIGEN_ALIGN32 VecNRf r;
 		JbBuffer_new[i].setZero();
 
 		// sum over all residuals.

src/FullSystem/CoarseInitializer.h

@@ -21,7 +21,7 @@
 * along with DSO. If not, see <http://www.gnu.org/licenses/>.
 */
 
-
+#define EIGEN_ALIGN32 EIGEN_ALIGN_TO_BOUNDARY(32)
 #pragma once
 
 #include "util/NumType.h"
@@ -142,7 +142,7 @@ class CoarseInitializer {
 			int lvl,
 			Mat88f &H_out, Vec8f &b_out,
 			Mat88f &H_out_sc, Vec8f &b_out_sc,
-			const SE3 &refToNew, AffLight refToNew_aff,
+			SE3 refToNew, AffLight refToNew_aff,
 			bool plot);
 	Vec3f calcEC(int lvl); // returns OLD NERGY, NEW ENERGY, NUM TERMS.
 	void optReg(int lvl);

src/FullSystem/CoarseTracker.cpp

@@ -289,7 +289,7 @@ void CoarseTracker::makeCoarseDepthL0(std::vector<FrameHessian*> frameHessians)
 
 
 
-void CoarseTracker::calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l)
+void CoarseTracker::calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, SE3 refToNew, AffLight aff_g2l)
 {
 	acc.initialize();
 
@@ -351,7 +351,7 @@ void CoarseTracker::calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &ref
 
 
 
-Vec6 CoarseTracker::calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH)
+Vec6 CoarseTracker::calcRes(int lvl, SE3 refToNew, AffLight aff_g2l, float cutoffTH)
 {
 	float E = 0;
 	int numTermsInE = 0;

src/FullSystem/CoarseTracker.h

@@ -96,10 +96,10 @@ class CoarseTracker {
 	float* weightSums_bak[PYR_LEVELS];
 
 
-	Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
-	Vec6 calcRes(int lvl, const SE3 &refToNew, AffLight aff_g2l, float cutoffTH);
-	void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);
-	void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, const SE3 &refToNew, AffLight aff_g2l);
+	Vec6 calcResAndGS(int lvl, Mat88 &H_out, Vec8 &b_out, SE3 refToNew, AffLight aff_g2l, float cutoffTH);
+	Vec6 calcRes(int lvl, SE3 refToNew, AffLight aff_g2l, float cutoffTH);
+	void calcGSSSE(int lvl, Mat88 &H_out, Vec8 &b_out, SE3 refToNew, AffLight aff_g2l);
+	void calcGS(int lvl, Mat88 &H_out, Vec8 &b_out, SE3 refToNew, AffLight aff_g2l);
 
 	// pc buffers
 	float* pc_u[PYR_LEVELS];

src/FullSystem/FullSystem.cpp

@@ -208,7 +208,7 @@ FullSystem::~FullSystem()
 	delete ef;
 }
 
-void FullSystem::setOriginalCalib(const VecXf &originalCalib, int originalW, int originalH)
+void FullSystem::setOriginalCalib(VecXf originalCalib, int originalW, int originalH)
 {
 
 }

src/FullSystem/FullSystem.h

@@ -25,7 +25,6 @@
 #pragma once
 #define MAX_ACTIVE_FRAMES 100
 
-#include <deque>
 #include "util/NumType.h"
 #include "util/globalCalib.h"
 #include "vector"
@@ -106,7 +105,7 @@ template<typename T> inline void deleteOutOrder(std::vector<T*> &v, const T* ele
 }
 
 
-inline bool eigenTestNan(const MatXX &m, std::string msg)
+inline bool eigenTestNan(MatXX m, std::string msg)
 {
 	bool foundNan = false;
 	for(int y=0;y<m.rows();y++)
@@ -131,7 +130,7 @@ inline bool eigenTestNan(const MatXX &m, std::string msg)
 
 class FullSystem {
 public:
-	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 	FullSystem();
 	virtual ~FullSystem();
 
@@ -160,7 +159,7 @@ class FullSystem {
 
 
 	void setGammaFunction(float* BInv);
-	void setOriginalCalib(const VecXf &originalCalib, int originalW, int originalH);
+	void setOriginalCalib(VecXf originalCalib, int originalW, int originalH);
 
 private:
 
@@ -206,7 +205,7 @@ class FullSystem {
 	void activatePointsMT_Reductor(std::vector<PointHessian*>* optimized,std::vector<ImmaturePoint*>* toOptimize,int min, int max, Vec10* stats, int tid);
 	void applyRes_Reductor(bool copyJacobians, int min, int max, Vec10* stats, int tid);
 
-	void printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b);
+	void printOptRes(Vec3 res, double resL, double resM, double resPrior, double LExact, float a, float b);
 
 	void debugPlotTracking();
 

src/FullSystem/FullSystemOptimize.cpp

@@ -391,7 +391,7 @@ double FullSystem::calcMEnergy()
 }
 
 
-void FullSystem::printOptRes(const Vec3 &res, double resL, double resM, double resPrior, double LExact, float a, float b)
+void FullSystem::printOptRes(Vec3 res, double resL, double resM, double resPrior, double LExact, float a, float b)
 {
 	printf("A(%f)=(AV %.3f). Num: A(%'d) + M(%'d); ab %f %f!\n",
 			res[0],

src/FullSystem/HessianBlocks.cpp

@@ -71,7 +71,7 @@ void PointHessian::release()
 }
 
 
-void FrameHessian::setStateZero(const Vec10 &state_zero)
+void FrameHessian::setStateZero(Vec10 state_zero)
 {
 	assert(state_zero.head<6>().squaredNorm() < 1e-20);
 

src/FullSystem/HessianBlocks.h

@@ -40,7 +40,7 @@ namespace dso
 {
 
 
-inline Vec2 affFromTo(const Vec2 &from, const Vec2 &to)	// contains affine parameters as XtoWorld.
+inline Vec2 affFromTo(Vec2 from, Vec2 to)	// contains affine parameters as XtoWorld.
 {
 	return Vec2(from[0] / to[0], (from[1] - to[1]) / to[0]);
 }
@@ -175,8 +175,8 @@ struct FrameHessian
 
 
 
-	void setStateZero(const Vec10 &state_zero);
-	inline void setState(const Vec10 &state)
+	void setStateZero(Vec10 state_zero);
+	inline void setState(Vec10 state)
 	{
 
 		this->state = state;
@@ -191,7 +191,7 @@ struct FrameHessian
 		PRE_camToWorld = PRE_worldToCam.inverse();
 		//setCurrentNullspace();
 	};
-	inline void setStateScaled(const Vec10 &state_scaled)
+	inline void setStateScaled(Vec10 state_scaled)
 	{
 
 		this->state_scaled = state_scaled;
@@ -206,7 +206,7 @@ struct FrameHessian
 		PRE_camToWorld = PRE_worldToCam.inverse();
 		//setCurrentNullspace();
 	};
-	inline void setEvalPT(const SE3 &worldToCam_evalPT, const Vec10 &state)
+	inline void setEvalPT(SE3 worldToCam_evalPT, Vec10 state)
 	{
 
 		this->worldToCam_evalPT = worldToCam_evalPT;
@@ -216,7 +216,7 @@ struct FrameHessian
 
 
 
-	inline void setEvalPT_scaled(const SE3 &worldToCam_evalPT, const AffLight &aff_g2l)
+	inline void setEvalPT_scaled(SE3 worldToCam_evalPT, AffLight aff_g2l)
 	{
 		Vec10 initial_state = Vec10::Zero();
 		initial_state[6] = aff_g2l.a;
@@ -343,7 +343,7 @@ struct CalibHessian
 
 
 
-	inline void setValue(const VecC &value)
+	inline void setValue(VecC value)
 	{
 		// [0-3: Kl, 4-7: Kr, 8-12: l2r]
 		this->value = value;
@@ -360,7 +360,7 @@ struct CalibHessian
 		this->value_minus_value_zero = this->value - this->value_zero;
 	};
 
-	inline void setValueScaled(const VecC &value_scaled)
+	inline void setValueScaled(VecC value_scaled)
 	{
 		this->value_scaled = value_scaled;
 		this->value_scaledf = this->value_scaled.cast<float>();

src/FullSystem/ImmaturePoint.cpp

@@ -72,7 +72,7 @@ ImmaturePoint::~ImmaturePoint()
  * * UPDATED -> point has been updated.
  * * SKIP -> point has not been updated.
  */
-ImmaturePointStatus ImmaturePoint::traceOn(FrameHessian* frame,const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f& hostToFrame_affine, CalibHessian* HCalib, bool debugPrint)
+ImmaturePointStatus ImmaturePoint::traceOn(FrameHessian* frame, Mat33f hostToFrame_KRKi, Vec3f hostToFrame_Kt, Vec2f hostToFrame_affine, CalibHessian* HCalib, bool debugPrint)
 {
 	if(lastTraceStatus == ImmaturePointStatus::IPS_OOB) return lastTraceStatus;
 

src/FullSystem/ImmaturePoint.h

@@ -81,7 +81,7 @@ class ImmaturePoint
 	ImmaturePoint(int u_, int v_, FrameHessian* host_, float type, CalibHessian* HCalib);
 	~ImmaturePoint();
 
-	ImmaturePointStatus traceOn(FrameHessian* frame, const Mat33f &hostToFrame_KRKi, const Vec3f &hostToFrame_Kt, const Vec2f &hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false);
+	ImmaturePointStatus traceOn(FrameHessian* frame, Mat33f hostToFrame_KRKi, Vec3f hostToFrame_Kt, Vec2f hostToFrame_affine, CalibHessian* HCalib, bool debugPrint=false);
 
 	ImmaturePointStatus lastTraceStatus;
 	Vec2f lastTraceUV;

src/IOWrapper/Output3DWrapper.h

@@ -127,7 +127,7 @@ class Output3DWrapper
          *  Calling:
          *  Always called, no overhead if not used.
          */
-        virtual void publishGraph(const std::map<uint64_t,Eigen::Vector2i, std::less<uint64_t>, Eigen::aligned_allocator<std::pair<uint64_t, Eigen::Vector2i> > > &connectivity) {}
+        virtual void publishGraph(const std::map<uint64_t,Eigen::Vector2i> &connectivity) {}
 
 
 

src/IOWrapper/Pangolin/PangolinDSOViewer.cpp

@@ -32,6 +32,10 @@
 #include "FullSystem/FullSystem.h"
 #include "FullSystem/ImmaturePoint.h"
 
+#ifdef HAS_ROS
+	#include <ros/ros.h>
+#endif
+
 namespace dso
 {
 namespace IOWrap
@@ -291,8 +295,12 @@ void PangolinDSOViewer::run()
 
 	printf("QUIT Pangolin thread!\n");
 	printf("I'll just kill the whole process.\nSo Long, and Thanks for All the Fish!\n");
-
-	exit(1);
+
+	#ifdef HAS_ROS
+		ros::shutdown();
+	#else
+		exit(1);
+	#endif
 }
 
 

src/OptimizationBackend/EnergyFunctional.h

@@ -114,11 +114,7 @@ class EnergyFunctional {
 	IndexThreadReduce<Vec10>* red;
 
 
-	std::map<uint64_t,
-	  Eigen::Vector2i,
-	  std::less<uint64_t>,
-	  Eigen::aligned_allocator<std::pair<uint64_t, Eigen::Vector2i>>
-	  > connectivityMap;
+    std::map<uint64_t,Eigen::Vector2i> connectivityMap;
 
 private:
 

src/OptimizationBackend/EnergyFunctionalStructs.h

@@ -84,8 +84,8 @@ class EFResidual
 
 	RawResidualJacobian* J;
 
-	VecNRf res_toZeroF;
-	Vec8f JpJdF;
+	EIGEN_ALIGN16 VecNRf res_toZeroF;
+	EIGEN_ALIGN16 Vec8f JpJdF;
 
 
 	// status.

src/OptimizationBackend/MatrixAccumulators.h

@@ -56,7 +56,7 @@ class AccumulatorXX
   }
 
 
-  inline void update(const Eigen::Matrix<float,i,1> &L, const Eigen::Matrix<float,j,1> &R, float w)
+  inline void update(Eigen::Matrix<float,i,1> L, Eigen::Matrix<float,j,1> R, float w)
   {
 	  A += w*L*R.transpose();
 	  numIn1++;
@@ -197,14 +197,14 @@ class AccumulatorX
   }
 
 
-  inline void update(const Eigen::Matrix<float,i,1> &L, float w)
+  inline void update(Eigen::Matrix<float,i,1> L, float w)
   {
 	  A += w*L;
 	  numIn1++;
 	  shiftUp(false);
   }
 
-  inline void updateNoWeight(const Eigen::Matrix<float,i,1> &L)
+  inline void updateNoWeight(Eigen::Matrix<float,i,1> L)
   {
 	  A += L;
 	  numIn1++;