Code formatting and inserting copyright notice

examples/vSLAMexample/Feature2D.cpp

@@ -1,2 +1,19 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Feature2D.cpp
+ * @brief
+ * @author Duy-Nguyen
+ */
+
 #include "Feature2D.h"
 

examples/vSLAMexample/Feature2D.h

@@ -1,28 +1,41 @@
-#ifndef FEATURE2D_H
+/* ----------------------------------------------------------------------------
-#define FEATURE2D_H
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Feature2D.h
+ * @brief
+ * @author Duy-Nguyen
+ */
+
+#pragma once
 
 #include "gtsam/geometry/Point2.h"
 #include <iostream>
 
-class Feature2D
+struct Feature2D
 {
-public:
+
   gtsam::Point2 m_p;
   int m_idCamera;            // id of the camera pose that makes this measurement
   int m_idLandmark;       // id of the 3D landmark that it is associated with
-public:
-    Feature2D(int idCamera, int idLandmark, gtsam::Point2 p)
-            :m_p(p),
-            m_idCamera(idCamera),
-            m_idLandmark(idLandmark)
-    {};
 
-    void print(const std::string& s = "") const
+  Feature2D(int idCamera, int idLandmark, gtsam::Point2 p) :
-    {
+    m_p(p),
+    m_idCamera(idCamera),
+    m_idLandmark(idLandmark) {}
+
+  void print(const std::string& s = "") const {
     std::cout << s << std::endl;
     std::cout << "Pose id: " << m_idCamera << " -- Landmark id: "  << m_idLandmark << std::endl;
     m_p.print("\tMeasurement: ");
   }
-};
 
-#endif // FEATURE2D_H
+};

examples/vSLAMexample/ISAMLoop-inl.h

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /*
  * ISAMLoop.cpp
  *

examples/vSLAMexample/ISAMLoop.h

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /*
  * ISAMLoop.h
  *

examples/vSLAMexample/vISAMexample.cpp

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file    vSLAMexample.cpp
  * @brief   An vSLAM example for synthesis sequence
@@ -54,8 +65,7 @@ SharedGaussian poseSigma(noiseModel::Unit::Create(1));
  * Read all data: calibration file, landmarks, poses, and all features measurements
  * Data is stored in global variables, which are used later to simulate incremental updates.
  */
-void readAllDataISAM()
+void readAllDataISAM() {
-{
   g_calib = readCalibData(g_dataFolder + CALIB_FILE);
 
   // Read groundtruth landmarks' positions. These will be used later as intial estimates and priors for landmark nodes.
@@ -73,14 +83,13 @@ void readAllDataISAM()
  * Setup newFactors and initialValues for each new pose and set of measurements at each frame.
  */
 void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
-                      int pose_id, Pose3& pose,
+    int pose_id, Pose3& pose, std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
-                      std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
+
-{
   // Create a graph of newFactors with new measurements
   newFactors = shared_ptr<Graph> (new Graph());
-    for (size_t i= 0; i<measurements.size(); i++)
+  for (size_t i = 0; i < measurements.size(); i++) {
-    {
+    newFactors->addMeasurement(
-        newFactors->addMeasurement(measurements[i].m_p,
+        measurements[i].m_p,
         measurementSigma,
         pose_id,
         measurements[i].m_idLandmark,
@@ -89,29 +98,24 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>&
 
   // ... we need priors on the new pose and all new landmarks
   newFactors->addPosePrior(pose_id, pose, poseSigma);
-    for (size_t i= 0; i<measurements.size(); i++)
+  for (size_t i = 0; i < measurements.size(); i++) {
-    {
     newFactors->addPointPrior(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
   }
 
-
   // Create initial values for all nodes in the newFactors
   initialValues = shared_ptr<Values> (new Values());
   initialValues->insert(pose_id, pose);
-    for (size_t i= 0; i<measurements.size(); i++)
+  for (size_t i = 0; i < measurements.size(); i++) {
-    {
     initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
   }
 }
 
-
 /* ************************************************************************* */
-int main(int argc, char* argv[])
+int main(int argc, char* argv[]) {
-{
+  if (argc < 2) {
-    if (argc <2)
-    {
     cout << "Usage: vISAMexample <DataFolder>" << endl << endl;
-        cout << "\tPlease specify <DataFolder>, which contains calibration file, initial landmarks, initial poses, and feature data." << endl;
+    cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
+      "landmarks, initial poses, and feature data." << endl;
     cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data/" << endl << endl;
     cout << "Example usage: vISAMexample '$gtsam_source_folder$/examples/vSLAMexample/Data/'" << endl;
     exit(0);
@@ -126,13 +130,10 @@ int main(int argc, char* argv[])
 
   // At each frame i with new camera pose and new set of measurements associated with it,
   // create a graph of new factors and update ISAM
-    for (size_t i = 0; i<g_measurements.size(); i++)
+  for (size_t i = 0; i < g_measurements.size(); i++) {
-    { 
     shared_ptr<Graph> newFactors;
     shared_ptr<Values> initialValues;
-        createNewFactors(newFactors, initialValues,
+    createNewFactors(newFactors, initialValues, i, g_poses[i], g_measurements[i], measurementSigma, g_calib);
-                         i, g_poses[i],
-                         g_measurements[i], measurementSigma, g_calib);
 
     isam.update(*newFactors, *initialValues);
     Values currentEstimate = isam.estimate();
@@ -140,7 +141,7 @@ int main(int argc, char* argv[])
     currentEstimate.print("Current estimate: ");
   }
 
+  return 0;
 }
 /* ************************************************************************* */
 
-

examples/vSLAMexample/vSFMexample.cpp

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
 /**
  * @file    vSLAMexample.cpp
  * @brief   An vSLAM example for synthesis sequence
@@ -47,8 +58,8 @@ SharedGaussian measurementSigma(noiseModel::Isotropic::Sigma(2, 5.0f));
  * Read all data: calibration file, landmarks, poses, and all features measurements
  * Data is stored in global variables.
  */
-void readAllData()
+void readAllData() {
-{
+
   g_calib = readCalibData(g_dataFolder + "/" + CALIB_FILE);
 
   // Read groundtruth landmarks' positions. These will be used later as intial estimates for landmark nodes.
@@ -67,16 +78,16 @@ void readAllData()
  * by adding and linking 2D features (measurements) detected in each captured image
  * with their corresponding landmarks.
  */
-Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib)
+Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measurementSigma, shared_ptrK calib) {
-{
+
   Graph g;
 
   cout << "Built graph: " << endl;
-    for (size_t i= 0; i<measurements.size(); i++)
+  for (size_t i = 0; i < measurements.size(); i++) {
-    {
     measurements[i].print();
 
-        g.addMeasurement(measurements[i].m_p,
+    g.addMeasurement(
+        measurements[i].m_p,
         measurementSigma,
         measurements[i].m_idCamera,
         measurements[i].m_idLandmark,
@@ -91,8 +102,8 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedGaussian measuremen
  * Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
  * The returned Values structure contains all initial values for all nodes.
  */
-Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses)
+Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
-{
+
   Values initValues;
 
   // Initialize landmarks 3D positions.
@@ -107,12 +118,11 @@ Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses)
 }
 
 /* ************************************************************************* */
-int main(int argc, char* argv[])
+int main(int argc, char* argv[]) {
-{
+  if (argc < 2) {
-    if (argc <2)
-    {
     cout << "Usage: vSFMexample <DataFolder>" << endl << endl;
-        cout << "\tPlease specify <DataFolder>, which contains calibration file, initial landmarks, initial poses, and feature data." << endl;
+    cout << "\tPlease specify <DataFolder>, which contains calibration file, initial\n"
+          "landmarks, initial poses, and feature data." << endl;
     cout << "\tSample folder is in $gtsam_source_folder$/examples/vSLAMexample/Data" << endl << endl;
     cout << "Example usage: vSFMexample '$gtsam_source_folder$/examples/vSLAMexample/Data'" << endl;
     exit(0);
@@ -143,6 +153,7 @@ int main(int argc, char* argv[])
   cout << "*******************************************************" << endl;
   result->print("FINAL RESULTS: ");
 
+  return 0;
 }
 /* ************************************************************************* */
 

examples/vSLAMexample/vSLAMutils.cpp

@@ -1,3 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file VSLAMutils.cpp
+ * @brief
+ * @author Duy-Nguyen
+ */
+
 #include "vSLAMutils.h"
 #include <fstream>
 #include <cstdio>
@@ -6,8 +23,7 @@ using namespace gtsam;
 using namespace std;
 
 /* ************************************************************************* */
-std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
+std::map<int, Point3> readLandMarks(const std::string& landmarkFile) {
-{
   ifstream file(landmarkFile.c_str());
   if (!file) {
     cout << "Cannot read landmark file: " << landmarkFile << endl;
@@ -18,8 +34,7 @@ std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
   file >> num;
   std::map<int, Point3> landmarks;
   landmarks.clear();
-    for (int i = 0; i<num; i++)
+  for (int i = 0; i<num; i++) {
-    {
     int color_id;
     float x, y, z;
     file >> color_id >> x >> y >> z;
@@ -35,11 +50,9 @@ std::map<int, Point3> readLandMarks(const std::string& landmarkFile)
  * Read pose from file, output by Panda3D.
  * Warning: row major!!!
  */
-gtsam::Pose3 readPose(const char* Fn)
+gtsam::Pose3 readPose(const char* Fn) {
-{
   ifstream poseFile(Fn);
-    if (!poseFile)
+  if (!poseFile) {
-    {
     cout << "Cannot read pose file: " << Fn << endl;
     exit(0);
   }
@@ -52,8 +65,7 @@ gtsam::Pose3 readPose(const char* Fn)
   // Because panda3d's camera is z-up, y-view,
   // we swap z and y to have y-up, z-view, then negate z to stick with the right-hand rule
   //... similar to OpenGL's camera
-    for (int i = 0; i<3; i++)
+  for (int i = 0; i<3; i++) {
-    {
     float t = v[4+i];
     v[4+i] = v[8+i];
     v[8+i] = -t;
@@ -67,19 +79,16 @@ gtsam::Pose3 readPose(const char* Fn)
   return pose;
 }
 /* ************************************************************************* */
-std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn)
+std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::string& posesFn) {
-{
   ifstream posesFile((baseFolder+posesFn).c_str());
-    if (!posesFile)
+  if (!posesFile) {
-    {
     cout << "Cannot read all pose file: " << posesFn << endl;
     exit(0);
   }
   int numPoses;
   posesFile >> numPoses;
   map<int, Pose3> poses;
-    for (int i = 0; i<numPoses; i++)
+  for (int i = 0; i<numPoses; i++) {
-    {
     int poseId;
     posesFile >> poseId;
 
@@ -94,11 +103,9 @@ std::map<int, gtsam::Pose3> readPoses(const std::string& baseFolder, const std::
 }
 
 /* ************************************************************************* */
-gtsam::shared_ptrK readCalibData(const std::string& calibFn)
+gtsam::shared_ptrK readCalibData(const std::string& calibFn) {
-{
   ifstream calibFile(calibFn.c_str());
-    if (!calibFile)
+  if (!calibFile) {
-    {
     cout << "Cannot read calib file: " << calibFn << endl;
     exit(0);
   }
@@ -109,12 +116,11 @@ gtsam::shared_ptrK readCalibData(const std::string& calibFn)
 
   return shared_ptrK(new Cal3_S2(fx, fy, 0, ox, oy));   // skew factor = 0
 }
+
 /* ************************************************************************* */
-std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
+std::vector<Feature2D> readFeatures(int pose_id, const char* filename) {
-{
   ifstream file(filename);
-    if (!file)
+  if (!file) {
-    {
     cout << "Cannot read feature file: " << filename<< endl;
     exit(0);
   }
@@ -123,8 +129,7 @@ std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
   file >> numFeatures ;
 
   std::vector<Feature2D> vFeatures_;
-    for (int i = 0; i < numFeatures; i++)
+  for (int i = 0; i < numFeatures; i++) {
-    {
     int landmark_id; double x, y;
     file >> landmark_id >> x >> y;
     vFeatures_.push_back(Feature2D(pose_id, landmark_id, Point2(x, y)));
@@ -133,12 +138,11 @@ std::vector<Feature2D> readFeatures(int pose_id, const char* filename)
   file.close();
   return vFeatures_;
 }
+
 /* ************************************************************************* */
-std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn)
+std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn) {
-{
   ifstream measurementsFile((baseFolder+measurementsFn).c_str());
-    if (!measurementsFile)
+  if (!measurementsFile) {
-    {
     cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
     exit(0);
   }
@@ -148,8 +152,7 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
   vector<Feature2D> allFeatures;
   allFeatures.clear();
 
-    for (int i = 0; i<numPoses; i++)
+  for (int i = 0; i<numPoses; i++) {
-    {
     int poseId;
     measurementsFile >> poseId;
 
@@ -161,20 +164,18 @@ std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const
 
   return allFeatures;
 }
+
 /* ************************************************************************* */
-std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn)
+std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std::string& posesFn) {
-{
   ifstream posesFile((baseFolder+posesFn).c_str());
-    if (!posesFile)
+  if (!posesFile) {
-    {
     cout << "Cannot read all pose ISAM file: " << posesFn << endl;
     exit(0);
   }
   int numPoses;
   posesFile >> numPoses;
   vector<Pose3> poses;
-    for (int i = 0; i<numPoses; i++)
+  for (int i = 0; i<numPoses; i++) {
-    {
     string poseFileName;
     posesFile >> poseFileName;
 
@@ -186,11 +187,9 @@ std::vector<gtsam::Pose3> readPosesISAM(const std::string& baseFolder, const std
 }
 
 /* ************************************************************************* */
-std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn)
+std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn) {
-{
   ifstream measurementsFile((baseFolder+measurementsFn).c_str());
-    if (!measurementsFile)
+  if (!measurementsFile) {
-    {
     cout << "Cannot read all pose file: " << baseFolder+measurementsFn << endl;
     exit(0);
   }
@@ -199,8 +198,7 @@ std::vector<std::vector<Feature2D> > readAllMeasurementsISAM(const std::string&
 
   std::vector<std::vector<Feature2D> > allFeatures;
 
-    for (int i = 0; i<numPoses; i++)
+  for (int i = 0; i<numPoses; i++) {
-    {
     string featureFileName;
     measurementsFile >> featureFileName;
     vector<Feature2D> features = readFeatures(-1, (baseFolder+featureFileName).c_str());    // we don't care about pose id in ISAM

examples/vSLAMexample/vSLAMutils.h

@@ -1,5 +1,21 @@
-#ifndef LANDMARKUTILS_H
+/* ----------------------------------------------------------------------------
-#define LANDMARKUTILS_H
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file Feature2D.cpp
+ * @brief
+ * @author Duy-Nguyen
+ */
+
+#pragma once
 
 #include <map>
 #include <vector>
@@ -21,7 +37,3 @@ gtsam::shared_ptrK readCalibData(const std::string& calibFn);
 std::vector<Feature2D> readFeatureFile(const char* filename);
 std::vector<Feature2D> readAllMeasurements(const std::string& baseFolder, const std::string& measurementsFn);
 std::vector< std::vector<Feature2D> > readAllMeasurementsISAM(const std::string& baseFolder, const std::string& measurementsFn);
-
-
-
-#endif // LANDMARKUTILS_H