started to create new observation struct for more compact ssa3d files

Author: Michael Ruhnke

ssa/types_3d/CMakeLists.txt

@@ -6,6 +6,8 @@ edge_se3_xyzcov.h
 edge_se3_xyzcov.cpp
 edge_xyzcov_xyzcov.h
 edge_xyzcov_xyzcov.cpp
+observation_point_xyz_rgb_normal.h
+observation_point_xyz_rgb_normal.cpp
 )
 
 SET_TARGET_PROPERTIES(types_3d PROPERTIES OUTPUT_NAME ${LIB_PREFIX}types_3d)

ssa/types_3d/observation_point_xyz_rgb_normal.cpp

@@ -0,0 +1,79 @@
+// Sparse Surface Optimization
+// Copyright (C) 2011 M. Ruhnke, R. Kuemmerle, G. Grisetti, W. Burgard
+//
+// SSA is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published
+// by the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// SSA is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+//#include <Eigen/Eigenvalues>
+#include "observation_point_xyz_rgb_normal.h"
+
+namespace ssa {
+  using namespace Eigen;
+
+  ObservationXYZRGBNormal::ObservationXYZRGBNormal() : measurement(Eigen::Vector3d::Zero()), estimate(Eigen::Vector3d::Zero()), normal(Eigen::Vector3d::Zero()), rgb(Eigen::Vector3i::Zero()), level(0)
+  {
+
+  }
+
+  ObservationXYZRGBNormal::ObservationXYZRGBNormal(Eigen::Vector3d& m, Eigen::Vector3d& e, Eigen::Vector3d& n, Eigen::Vector3i& color, int l) : measurement(m), estimate(e), normal(n), rgb(color), level(l)
+  {
+
+  };
+
+
+  bool ObservationXYZRGBNormal::read(std::istream& is)
+  {
+    /// measurement in sensor frame
+    for(int i = 0; i < 3; ++i)
+      is >> measurement(i);
+
+   /// estimate in global frame
+    for(int i = 0; i < 3; ++i)
+      is >> estimate(i);
+
+    /// normal in local frame
+    for(int i = 0; i < 3; ++i)
+      is >> normal(i);
+
+    /// color as r,g,b
+    for(int i = 0; i < 3; ++i)
+      is >> rgb(i);
+
+    is >> level;
+    return true;
+  }
+
+  bool ObservationXYZRGBNormal::write(std::ostream& os) const
+  {
+    /// measurement in sensor frame
+    for(int i = 0; i < 3; ++i)
+      os << measurement(i) << " ";
+
+    /// estimate in global frame
+    for(int i = 0; i < 3; ++i)
+      os << estimate(i) << " ";
+
+    /// normal in local frame
+    for(int i = 0; i < 3; ++i)
+      os << normal(i) << " ";
+
+    /// color as r,g,b
+    for(int i = 0; i < 3; ++i)
+      os << rgb(i) << " ";
+
+    os << level << " ";
+    return os.good();
+  }
+
+} //end namespace
+

ssa/types_3d/observation_point_xyz_rgb_normal.h

@@ -0,0 +1,52 @@
+#ifndef __SSA_OBSERVATION_XYZ_RGB_NORMAL__
+#define __SSA_OBSERVATION_XYZ_RGB_NORMAL__
+
+// Sparse Surface Optimization
+// Copyright (C) 2011 M. Ruhnke, R. Kuemmerle, G. Grisetti, W. Burgard
+//
+// SSA is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published
+// by the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// SSA is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#include <Eigen/Core>
+
+namespace ssa {
+
+  /** \brief: SSA class for single observation of a surface point
+   *
+   * This point vertices represent the observed surface,
+   * which will be adapted during optimization.
+   *
+   */
+
+  class ObservationXYZRGBNormal
+  {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+      Eigen::Vector3d measurement;
+      Eigen::Vector3d estimate;
+      Eigen::Vector3d normal;
+      Eigen::Vector3i rgb;
+      unsigned short level;
+
+
+      ObservationXYZRGBNormal();
+      ObservationXYZRGBNormal(Eigen::Vector3d& measurement, Eigen::Vector3d& estimate, Eigen::Vector3d& normal, Eigen::Vector3i& rgb, int level = 0);
+
+      bool read(std::istream& is);
+      bool write(std::ostream& os) const;
+
+  };
+
+}
+#endif