Merge pull request #3 from moriarty/melodic-devel

[melodic-devel][18.04] fix compile errors
ros-perception · Aug 19, 2018 · d0dfb48 · d0dfb48
2 parents 804d335 + cd63c6b
commit d0dfb48
Show file tree

Hide file tree

Showing 9 changed files with 35 additions and 34 deletions.
diff --git a/include/sparse_bundle_adjustment/proj.h b/include/sparse_bundle_adjustment/proj.h
@@ -128,7 +128,7 @@ namespace sba
 
       /// scaling factor for quaternion derivatives relative to translational ones;
       /// not sure if this is needed, it's close to 1.0
-      const static double qScale = 1.0;
+      static constexpr double qScale = 1.0;
 
       /// Use a covariance matrix?
       bool useCovar;

diff --git a/include/sparse_bundle_adjustment/sba.h b/include/sparse_bundle_adjustment/sba.h
@@ -325,7 +325,7 @@ namespace sba
 
     /// scaling factor for quaternion derivatives relative to translational ones;
     /// not sure if this is needed, it's close to 1.0
-    const static double qScale = 1.0;
+    static constexpr double qScale = 1.0;
 
     /// dpc/dq = dR'/dq [pw-t], in homogeneous form, with q a quaternion param
     /// 

diff --git a/include/sparse_bundle_adjustment/spa2d.h b/include/sparse_bundle_adjustment/spa2d.h
@@ -168,7 +168,7 @@ namespace sba
 
     /// scaling factor for quaternion derivatives relative to translational ones;
     /// not sure if this is needed, it's close to 1.0
-    const static double qScale = 1.0;
+    static constexpr double qScale = 1.0;
 
     /// dpc/dq = dR'/dq [pw-t], in homogeneous form, with q a quaternion param
     /// 

diff --git a/src/node.cpp b/src/node.cpp
@@ -42,7 +42,7 @@ namespace sba
     if (sn >= 0.9999)            // too close to high derivatives
       qrot.coeffs().head<3>() *= -1.0/(sqrt(sn)*1.0001); // switch sides; 1e-4 seems to work well
     qrot.w() = sqrt(1.0 - qrot.coeffs().head<3>().squaredNorm());
-    if (isnan(qrot.x()) || isnan(qrot.y()) || isnan(qrot.z()) || isnan(qrot.w()) )
+    if (std::isnan(qrot.x()) || std::isnan(qrot.y()) || std::isnan(qrot.z()) || std::isnan(qrot.w()) )
       { 
         printf("[NormRot] Bad quaternion: %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w()); 
         exit(1); 
@@ -123,7 +123,7 @@ namespace sba
   {
       qrot.normalize();
       if (qrot.w() < 0) qrot.coeffs().head<3>() = -qrot.coeffs().head<3>();
-      if (isnan(qrot.x()) || isnan(qrot.y()) || isnan(qrot.z()) || isnan(qrot.w()) )
+      if (std::isnan(qrot.x()) || std::isnan(qrot.y()) || std::isnan(qrot.z()) || std::isnan(qrot.w()) )
         { 
           printf("[NormRot] Bad quaternion in normRotLocal(): %f %f %f %f\n", qrot.x(), qrot.y(), qrot.z(), qrot.w());
           exit(1); 

diff --git a/src/proj.cpp b/src/proj.cpp
@@ -74,7 +74,7 @@ namespace sba
     double py = pc(1);
     double pz = pc(2);
     double ipz2 = 1.0/(pz*pz);
-    if (isnan(ipz2) ) { printf("[SetJac] infinite jac\n");  *(int *)0x0 = 0; }
+    if (std::isnan(ipz2) ) { printf("[SetJac] infinite jac\n");  *(int *)0x0 = 0; }
 
     double ipz2fx = ipz2*nd.Kcam(0,0); // Fx
     double ipz2fy = ipz2*nd.Kcam(1,1); // Fy
@@ -125,7 +125,7 @@ namespace sba
 #ifdef DEBUG
     for (int i=0; i<2; i++)
       for (int j=0; j<6; j++)
-        if (isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j);  *(int *)0x0 = 0; }
+        if (std::isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j);  *(int *)0x0 = 0; }
 #endif
 
     // Set Hessians + extras.
@@ -148,7 +148,7 @@ namespace sba
     {
 #ifdef DEBUG
       printf("[CalcErr] negative Z! Node %d \n",ndi);
-      if (isnan(err[0]) || isnan(err[1]) ) printf("[CalcErr] NaN!\n"); 
+      if (std::isnan(err[0]) || std::isnan(err[1]) ) printf("[CalcErr] NaN!\n");
 #endif
       err = Eigen::Vector3d(0.0,0.0,0.0);
       return 0.0;
@@ -204,7 +204,7 @@ namespace sba
     double py = pc(1);
     double pz = pc(2);
     double ipz2 = 1.0/(pz*pz);
-    if (isnan(ipz2) ) { printf("[SetJac] infinite jac\n");  *(int *)0x0 = 0; }
+    if (std::isnan(ipz2) ) { printf("[SetJac] infinite jac\n");  *(int *)0x0 = 0; }
 
     double ipz2fx = ipz2*nd.Kcam(0,0); // Fx
     double ipz2fy = ipz2*nd.Kcam(1,1); // Fy
@@ -265,7 +265,7 @@ namespace sba
 #ifdef DEBUG
     for (int i=0; i<2; i++)
       for (int j=0; j<6; j++)
-        if (isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j);  *(int *)0x0 = 0; }
+        if (std::isnan(jacc(i,j)) ) { printf("[SetJac] NaN in jacc(%d,%d)\n", i, j);  *(int *)0x0 = 0; }
 #endif
     if (useCovar)
     {
@@ -317,7 +317,7 @@ namespace sba
     {
 #ifdef DEBUG
       printf("[CalcErr] negative Z! Node %d\n",ndi);
-      if (isnan(err[0]) || isnan(err[1]) ) printf("[CalcErr] NaN!\n"); 
+      if (std::isnan(err[0]) || std::isnan(err[1]) ) printf("[CalcErr] NaN!\n");
 #endif
       err.setZero();
 

diff --git a/src/sba.cpp b/src/sba.cpp
@@ -1141,10 +1141,10 @@ void SysSBA::setupSys(double sLambda)
     // check the matrix and vector
     for (int i=0; i<6*nFree; i++)
       for (int j=0; j<6*nFree; j++)
-        if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
+        if (std::isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
 
     for (int j=0; j<6*nFree; j++)
-      if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
+      if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
 
     int ndc = 0;
     for (int i=0; i<nFree; i++)

diff --git a/src/sba_file_io.cpp b/src/sba_file_io.cpp
@@ -114,7 +114,7 @@ int sba::readBundlerFile(const char *filename, SysSBA& sbaout)
 int sba::writeBundlerFile(const char *filename, SysSBA& sbain)
 {
     ofstream outfile(filename, ios_base::trunc);
-    if (outfile == NULL)
+    if (!outfile)
     {
         cout << "Can't open file " << filename << endl;
         return -1;
@@ -204,7 +204,7 @@ int  sba::ParseBundlerFile(const char *fin,	// input file
 		)
 {
     ifstream ifs(fin);
-    if (ifs == NULL)
+    if (!ifs)
       {
         cout << "Can't open file " << fin << endl;
         return -1;
@@ -324,7 +324,7 @@ void sba::writeLourakisFile(const char *fname, SysSBA& sba)
     char name[1024];
     sprintf(name,"%s-cams.txt",fname);
     FILE *fn = fopen(name,"w");
-    if (fn == NULL)
+    if (fn == nullptr)
       {
         cout << "[WriteFile] Can't open file " << name << endl;
         return;
@@ -346,7 +346,7 @@ void sba::writeLourakisFile(const char *fname, SysSBA& sba)
 
     sprintf(name,"%s-pts.txt",fname);
     fn = fopen(name,"w");
-    if (fn == NULL)
+    if (fn == nullptr)
       {
         cout << "[WriteFile] Can't open file " << name << endl;
         return;
@@ -382,7 +382,7 @@ void sba::writeLourakisFile(const char *fname, SysSBA& sba)
     // write camera calibartion
     sprintf(name,"%s-calib.txt",fname);
     fn = fopen(name,"w");
-    if (fn == NULL)
+    if (fn == nullptr)
       {
         cout << "[WriteFile] Can't open file " << name << endl;
         return;
@@ -401,7 +401,7 @@ void sba::writeLourakisFile(const char *fname, SysSBA& sba)
 void sba::writeA(const char *fname, SysSBA& sba)
 {
     ofstream ofs(fname);
-    if (ofs == NULL)
+    if (!ofs)
       {
         cout << "Can't open file " << fname << endl;
         return;
@@ -422,7 +422,7 @@ void sba::writeSparseA(const char *fname, SysSBA& sba)
 
     {
       ofstream ofs(name);
-      if (ofs == NULL)
+      if (!ofs)
         {
           cout << "Can't open file " << fname << endl;
           return;
@@ -462,7 +462,7 @@ void sba::writeSparseA(const char *fname, SysSBA& sba)
 
     {
       ofstream ofs(name);
-      if (ofs == NULL)
+      if (!ofs)
         {
           cout << "Can't open file " << fname << endl;
           return;
@@ -620,7 +620,7 @@ int  sba::ParseGraphFile(const char *fin,	// input file
 {
   // input stream
   ifstream ifs(fin);
-  if (ifs == NULL)
+  if (!ifs)
     {
       cout << "Can't open file " << fin << endl;
       return -1;
@@ -783,7 +783,7 @@ int  sba::ParseGraphFile(const char *fin,	// input file
 int sba::writeGraphFile(const char *filename, SysSBA& sba, bool mono)
 {
     ofstream outfile(filename, ios_base::trunc);
-    if (outfile == NULL)
+    if (!outfile)
     {
         cout << "Can't open file " << filename << endl;
         return -1;
@@ -1002,7 +1002,7 @@ sba::ParseSPAGraphFile(const char *fin, // input file
    std::vector< Eigen::Matrix<double,6,6>, Eigen::aligned_allocator<Eigen::Matrix<double,6,6> > > &prec) // constraint covariance
 {
   ifstream ifs(fin);
-  if (ifs == NULL)
+  if (!ifs)
     {
       cout << "Can't open file " << fin << endl;
       return -1;

diff --git a/src/spa.cpp b/src/spa.cpp
@@ -60,12 +60,13 @@ using namespace std;
 #include <iomanip>
 #include <fstream>
 #include <sys/time.h>
+#include <utility>
 
 // elapsed time in microseconds
 static long long utime()
 {
   timeval tv;
-  gettimeofday(&tv,NULL);
+  gettimeofday(&tv,nullptr);
   long long ts = tv.tv_sec;
   ts *= 1000000;
   ts += tv.tv_usec;
@@ -87,7 +88,7 @@ namespace sba
     //                    ; (p1-p2)^2 = s1*K_12 with weight w
 
     ifstream ifs(fname);
-    if (ifs == NULL)
+    if (!ifs)
       {
         cout << "Can't open file " << fname << endl;
         return false;
@@ -836,10 +837,10 @@ namespace sba
     // check the matrix and vector
     for (int i=0; i<6*nFree; i++)
       for (int j=0; j<6*nFree; j++)
-        if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
+        if (std::isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
 
     for (int j=0; j<6*nFree; j++)
-      if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
+      if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
 
     int ndc = 0;
     for (int i=0; i<nFree; i++)
@@ -1127,7 +1128,7 @@ namespace sba
   void SysSPA::writeSparseA(char *fname, bool useCSparse)
   {
     ofstream ofs(fname);
-    if (ofs == NULL)
+    if (!ofs)
       {
         cout << "Can't open file " << fname << endl;
         return;
@@ -1191,8 +1192,8 @@ namespace sba
     if (node >= nnodes)
       node = 0;
     dist[node] = 0.0;
-    multimap<double,int> open;  // open list, priority queue - can have duplicates
-    open.insert(make_pair<double,int>(0.0,node));
+    std::multimap<double,int> open;  // open list, priority queue - can have duplicates
+    open.emplace(0.0,node);
 
     // do breadth-first computation
     while (!open.empty())
@@ -1230,7 +1231,7 @@ namespace sba
               {
                 // set priority queue
                 dist[nn] = di+dd;
-                open.insert(make_pair<double,int>(di+dd,nn));
+                open.emplace(di+dd,nn);
                 // update initial pose
                 Vector4d trans;
                 trans.head(3) = tmean;

diff --git a/src/spa2d.cpp b/src/spa2d.cpp
@@ -313,10 +313,10 @@ namespace sba
     // check the matrix and vector
     for (int i=0; i<3*nFree; i++)
       for (int j=0; j<3*nFree; j++)
-        if (isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
+        if (std::isnan(A(i,j)) ) { printf("[SetupSys] NaN in A\n"); *(int *)0x0 = 0; }
 
     for (int j=0; j<3*nFree; j++)
-      if (isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
+      if (std::isnan(B[j]) ) { printf("[SetupSys] NaN in B\n"); *(int *)0x0 = 0; }
 
     int ndc = 0;
     for (int i=0; i<nFree; i++)