voxelhashing export poses

[luozhipi]

do i need, modify codes, or just use the dumping .matrix files?

[robmaier]

Hi, here is the patch for the changes I have made to VoxelHashing in order to export the trajectory in the proper format:

diff --git a/DepthSensingCUDA/Source/CUDARGBDAdapter.h b/DepthSensingCUDA/Source/CUDARGBDAdapter.h
index d1d7684..daada96 100644
--- a/DepthSensingCUDA/Source/CUDARGBDAdapter.h
+++ b/DepthSensingCUDA/Source/CUDARGBDAdapter.h
@@ -99,6 +99,10 @@ class CUDARGBDAdapter
            m_RGBDSensor->saveRecordedFramesToFile(filename);
        }

+       void saveRecordedTrajectoryToFile(const std::string& filename) {
+           m_RGBDSensor->saveRecordedTrajectoryToFile(filename);
+       }
+
        void recordFrame() {
            m_RGBDSensor->recordFrame();
        }
diff --git a/DepthSensingCUDA/Source/DepthSensing.cpp b/DepthSensingCUDA/Source/DepthSensing.cpp
index cdf7617..8f65ff2 100644
--- a/DepthSensingCUDA/Source/DepthSensing.cpp
+++ b/DepthSensingCUDA/Source/DepthSensing.cpp
@@ -443,6 +443,12 @@ void CALLBACK OnKeyboard( UINT nChar, bool bKeyDown, bool bAltDown, void* pUserC
                if (GlobalAppState::get().s_integrationEnabled)     std::cout << "integration enabled" << std::endl;
                else std::cout << "integration disabled" << std::endl;
            }
+       case 'J':
+       {
+           std::cout << "save trajectory file..." << std::endl;
+           std::string filenamePoses = "./trajectory.txt";
+           g_RGBDAdapter.saveRecordedTrajectoryToFile(filenamePoses);
+       }

        default:
            break;
diff --git a/DepthSensingCUDA/Source/RGBDSensor.cpp b/DepthSensingCUDA/Source/RGBDSensor.cpp
index 945050c..7fd8354 100644
--- a/DepthSensingCUDA/Source/RGBDSensor.cpp
+++ b/DepthSensingCUDA/Source/RGBDSensor.cpp
@@ -443,6 +443,34 @@ void RGBDSensor::saveRecordedFramesToFile( const std::string& filename )
 }

+void RGBDSensor::saveRecordedTrajectoryToFile(const std::string& filename)
+{
+   if (!m_recordedDataCache || !m_recordedData)
+       return;
+
+   std::ofstream posesFile(filename.c_str());
+   posesFile << std::fixed;
+
+   //setting the trajectory first
+   for (size_t i = 0; i < m_recordedTrajectory.size(); i++) {
+       mat4f pose = m_recordedTrajectory[i];
+
+       //write timestamp and pose into poses file
+       double timePose = (double)i;
+       posesFile << timePose << " ";
+       // convert pose
+       //translation
+       vec3f translation = pose.getTranslation();
+       posesFile << translation[0] << " " << translation[1] << " " << translation[2];
+       //rotation (quaternion)
+       mat3f rot = pose.getRotation();
+       Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rotEigen(rot.getRawData());
+       Eigen::Quaternionf quat(rotEigen);
+       posesFile << " " << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w() << std::endl;
+   }
+   posesFile.close();
+}
+

 ml::vec3f RGBDSensor::depthToSkeleton(unsigned int ux, unsigned int uy) const
 {
diff --git a/DepthSensingCUDA/Source/RGBDSensor.h b/DepthSensingCUDA/Source/RGBDSensor.h
index 3effd69..b59124c 100644
--- a/DepthSensingCUDA/Source/RGBDSensor.h
+++ b/DepthSensingCUDA/Source/RGBDSensor.h
@@ -94,6 +94,9 @@ public:
    //! saves all previously recorded frames to file
    void saveRecordedFramesToFile(const std::string& filename);

+   //! saves recorded trajectory to file
+   void saveRecordedTrajectoryToFile(const std::string& filename);
+
    //! returns the current rigid transform; if not specified by the 'actual' sensor the identiy is returned
    virtual mat4f getRigidTransform(int offset) const {
        return mat4f::identity();