small changes

ptam/CMakeLists.txt

@@ -110,6 +110,9 @@ target_link_libraries(ptam ${PTAM_LIBRARIES} ${OpenCV_LIBS})
 # Camera calibrator 
 rosbuild_add_executable(cameracalibrator ${CALIBRATOR_FILES})
 target_link_libraries(cameracalibrator ${PTAM_LIBRARIES} ${OpenCV_LIBS})
+rosbuild_link_boost(cameracalibrator filesystem)
+rosbuild_link_boost(cameracalibrator system)
+
 
 # PTAM remote control
 # this is unfortunately neccessary to close app, when opencvwindow gets closed

ptam/Makefile

@@ -2,8 +2,10 @@ all:
 	cd thirdparty && make
 	make -f Makefile_ptam
 
+eclipse-project:
+	make -f Makefile_ptam $@
+
 .DEFAULT:
 	cd thirdparty && make $@
 	make -f Makefile_ptam $@
 
-

ptam/src/ATANCamera.cc

@@ -17,6 +17,12 @@ ATANCamera::ATANCamera(string sName)
 	//mgvvCameraParams = mvDefaultParams;
 	FixParams* pPars = ParamsAccess::fixParams;
 	mgvvCameraParams = makeVector(pPars->Cam_fx, pPars->Cam_fy, pPars->Cam_cx, pPars->Cam_cy, pPars->Cam_s);
+
+
+	printf("Camera Parameters: fx: %f, fy: %f, cx: %f, cy: %f, dist: %f\n",
+	  pPars->Cam_fx, pPars->Cam_fy, pPars->Cam_cx, pPars->Cam_cy, pPars->Cam_s);
+
+
 	//GV2.Register(mgvvCameraParams, sName+".Parameters", mvDefaultParams, HIDDEN | FATAL_IF_NOT_DEFINED);
   //mvImageSize[0] = 640.0;
   //mvImageSize[1] = 480.0;

ptam/src/CalibCornerPatch.cc

@@ -150,6 +150,13 @@ double CalibCornerPatch::Iterate(Image<byte> &im)
     {
       Vector<2> v2Pos_Template = vec(ir) - vec(mimTemplate.size() - ImageRef(1,1)) / 2.0;
       Vector<2> v2Pos_Image = mParams.v2Pos + v2Pos_Template;
+      if(!(v2Pos_Image[0] >= 0) || !(v2Pos_Image[1] >= 0))
+      {
+    	  std::cout << "cought NAN, prevented segfault!\n";
+    	  v2Pos_Image[0] = v2Pos_Image[1] = 100;
+    	  return 1e10;
+      }
+
       double dDiff = imInterp[v2Pos_Image] - (mParams.dGain * mimTemplate[ir] + mParams.dMean);
       dSum += fabs(dDiff);
       Vector<6> v6Jac;

ptam/src/CameraCalibrator.cc

@@ -15,6 +15,9 @@
 #include <ros/ros.h>
 #include <ros/package.h>
 
+#include <cv_bridge/cv_bridge.h>
+#include <opencv/cv.h>
+
 using namespace CVD;
 using namespace std;
 using namespace GVars3;
@@ -56,14 +59,34 @@ int main(int argc, char** argv)
 void CameraCalibrator::imageCallback(const sensor_msgs::ImageConstPtr & img)
 {
 
-  ROS_ASSERT(img->encoding == sensor_msgs::image_encodings::MONO8 && img->step == img->width);
+//  ROS_ASSERT(img->step == img->width);
+
+  cv::Mat imgBW;
+  if(img->encoding == sensor_msgs::image_encodings::RGB8)
+  {
+      // wrap around color data, and convert to bw image.
+      cv::Mat imgColTmp = cv::Mat(img->height, img->width,CV_8UC3, (uchar*)img->data.data());
+      cv::cvtColor(imgColTmp,imgBW,CV_RGB2GRAY);
+  }
+  else if(img->encoding == sensor_msgs::image_encodings::MONO8)
+  {
+      // just wrap around data... does not copy any image data.
+      imgBW = cv::Mat(img->height, img->width,CV_8U, (uchar*)img->data.data());
+  }
+  else
+  {
+    // throw error
+    ROS_ASSERT(img->encoding == sensor_msgs::image_encodings::RGB8 || img->encoding == sensor_msgs::image_encodings::MONO8);
+  }
+
+  // resize internal image if required
+	if(mCurrentImage.size().x != img->width || mCurrentImage.size().y != img->height)
+		mCurrentImage.resize(CVD::ImageRef(img->width, img->height));
 
-  CVD::ImageRef size(img->width, img->height);
-  mCurrentImage.resize(size);
+  // copy data
+	memcpy(mCurrentImage.data(),imgBW.data,img->width * img->height);
 
-  CVD::BasicImage<CVD::byte> img_tmp((CVD::byte *)&img->data[0], size);
-  CVD::copy(img_tmp, mCurrentImage);
-  mNewImage = true;
+	mNewImage = true;
 }
 
 CameraCalibrator::CameraCalibrator() :
@@ -83,6 +106,9 @@ void CameraCalibrator::init()
 {
   mGLWindow = new GLWindow2(mCurrentImage.size(), "Camera Calibrator");
 
+  mbDone = false;
+
+  GUI.RegisterCommand("CameraCalibrator.GrabImages", GUICommandCallBack, this);
   GUI.RegisterCommand("CameraCalibrator.GrabNextFrame", GUICommandCallBack, this);
   GUI.RegisterCommand("CameraCalibrator.Reset", GUICommandCallBack, this);
   GUI.RegisterCommand("CameraCalibrator.ShowNext", GUICommandCallBack, this);
@@ -95,6 +121,7 @@ void CameraCalibrator::init()
   GV3::Register(mgvnDisableDistortion, "CameraCalibrator.NoDistortion", 0, SILENT);
 
   GUI.ParseLine("GLWindow.AddMenu CalibMenu");
+  GUI.ParseLine("CalibMenu.AddMenuButton Live GrabImages CameraCalibrator.GrabImages");
   GUI.ParseLine("CalibMenu.AddMenuButton Live GrabFrame CameraCalibrator.GrabNextFrame");
   GUI.ParseLine("CalibMenu.AddMenuButton Live Reset CameraCalibrator.Reset");
   GUI.ParseLine("CalibMenu.AddMenuButton Live Optimize \"CameraCalibrator.Optimize=1\"");
@@ -218,6 +245,25 @@ void CameraCalibrator::GUICommandCallBack(void* ptr, string sCommand, string sPa
   ((CameraCalibrator*)ptr)->GUICommandHandler(sCommand, sParams);
 }
 
+
+#include <opencv/highgui.h>
+#include <opencv/cxcore.h>
+#include <opencv/cv.h>
+#include "boost/filesystem/operations.hpp"
+#include "boost/filesystem/path.hpp"
+namespace fs = boost::filesystem;
+
+void conversionCVToNB(cv::Mat frame, Image<byte> &imBW, ATANCamera& mCamera){
+	cv::Mat clone = frame.clone();
+	cv::Mat_<cv::Vec3b>& frame_p = (cv::Mat_<cv::Vec3b>&)clone;
+	for (int i = 0; i < mCamera.GetImageSize()[1]; i++){
+		for (int j = 0; j < mCamera.GetImageSize()[0]; j++){	
+		imBW[i][j] = (frame_p(i,j)[0] + frame_p(i,j)[1] + frame_p(i,j)[2]) / 3;
+		}
+	}
+
+}
+
 void CameraCalibrator::GUICommandHandler(string sCommand, string sParams) // Called by the callback func..
 {
   if (sCommand == "CameraCalibrator.Reset")
@@ -255,6 +301,89 @@ void CameraCalibrator::GUICommandHandler(string sCommand, string sParams) // Cal
     }
     mbDone = true;
   }
+if(sCommand=="CameraCalibrator.GrabImages")
+    {
+      fs::path full_path( fs::initial_path<fs::path>() );
+      full_path = fs::system_complete( fs::path( "Images/" ) );
+
+      if ( !fs::exists( full_path ) )
+      {
+	std::cout << "\nNot found: " << full_path.c_str() << std::endl;
+	return;
+      }
+
+      if ( fs::is_directory( full_path ) )
+      {
+	std::cout << "\nIn directory: "
+		  << full_path.c_str() << "\n\n";
+	fs::directory_iterator end_iter;
+	for ( fs::directory_iterator dir_itr( full_path );
+	      dir_itr != end_iter;
+	      ++dir_itr )
+	{
+	  try
+	  {
+	    if ( fs::is_directory( dir_itr->status() ) )
+	    {
+// 	      ++dir_count;
+// 	      std::cout << dir_itr->path().filename() << " [directory]\n";
+	    }
+	    else if ( fs::is_regular_file( dir_itr->status() ) )
+	    {
+// 	      ++file_count;
+// 	      std::cout << dir_itr->path().filename() << "\n";
+
+	      cv::Mat image;
+	      image = cv::imread(dir_itr->path().c_str());
+	      printf("path: %s\n", dir_itr->path().c_str());
+// 	      cv::Mat grayImage;
+// 	      cvtColor(image, grayImage, CV_RGB2GRAY);
+// 	      printf("1\n");
+
+	      Image<byte> imBW(mCurrentImage.size());
+	      conversionCVToNB(image, imBW, mCamera);
+
+		  // TODO: display 3D points
+		  glDrawPixels(imBW);
+
+	      CalibImage c;
+	      if(c.MakeFromImage(imBW))
+	      {
+		printf("Using image %s\n", dir_itr->path().filename().c_str());
+		mvCalibImgs.push_back(c);
+		mvCalibImgs.back().GuessInitialPose(mCamera);
+
+		// TODO: display 3D points
+		//mvCalibImgs.back().Draw3DGrid(mCamera, false);
+	    mGLWindow->HandlePendingEvents();
+	    mGLWindow->swap_buffers();
+		//sleep(3);
+	      }
+	      else
+	      {
+		printf("NOT using image %s\n", dir_itr->path().filename().c_str());
+	      }
+	    }
+	    else
+	    {
+// 	      ++other_count;
+// 	      std::cout << dir_itr->path().filename() << " [other]\n";
+	    }
+
+	  }
+	  catch ( const std::exception & ex )
+	  {
+// 	    ++err_count;
+	    std::cout << dir_itr->path().filename().c_str() << " " << ex.what() << std::endl;
+	  }
+	}
+// 	std::cout << "\n" << file_count << " files\n"
+// 		  << dir_count << " directories\n"
+// 		  << other_count << " others\n"
+// 		  << err_count << " errors\n";
+      }
+
+    }
   if (sCommand == "exit" || sCommand == "quit")
   {
     mbDone = true;

ptam/src/MapMaker.cc

@@ -452,11 +452,11 @@ bool MapMaker::InitFromStereo(KeyFrame &kF,
 	}
 
 	//sanity check: if the point variance is too large assume init is crap --> very hacky, assumes flat init scene!!
-	if((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0>0.5)
-	{
-		ROS_WARN_STREAM("Initial map rejected because of too large point variance. Point sigma: " << ((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0));
-		return false;
-	}
+	//if((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0>0.5)
+	//{
+	//	ROS_WARN_STREAM("Initial map rejected because of too large point variance. Point sigma: " << ((pkFirst->dSceneDepthSigma+pkSecond->dSceneDepthSigma)/2.0));
+	//	return false;
+	//}
 
 //}
 

ptam/src/Params.cpp

@@ -30,7 +30,7 @@ void FixParams::readFixParams()
 //    ROS_BREAK();
   }
 
-  nh.param("ImageSizeX", ImageSizeX, 640);
+  nh.param("ImageSizeX", ImageSizeX, 752);
   nh.param("ImageSizeY", ImageSizeY, 480);
   nh.param("ARBuffer_width", ARBuffer_width, 1200);
   nh.param("ARBuffer_height", ARBuffer_height, 900);
@@ -39,11 +39,11 @@ void FixParams::readFixParams()
   nh.param("TrackerMEstimator", TrackerMEstimator, std::string("Tukey"));
   nh.param("MinTukeySigma", MinTukeySigma, 0.4);
   nh.param("CandidateMinSTScore", CandidateMinSTScore, 70);
-  //	nh.param("Cam_fx", Cam_fx,0.392);
-  //	nh.param("Cam_fy", Cam_fy,0.613);
-  //	nh.param("Cam_cx", Cam_cx,0.484);
-  //	nh.param("Cam_cy", Cam_cy,0.476);
-  //	nh.param("Cam_s", Cam_s,0.967);
+	nh.param("Cam_fx", Cam_fx,0.392);
+	nh.param("Cam_fy", Cam_fy,0.613);
+	nh.param("Cam_cx", Cam_cx,0.484);
+	nh.param("Cam_cy", Cam_cy,0.476);
+	nh.param("Cam_s", Cam_s,0.967);
   nh.param("Calibrator_BlurSigma", Calibrator_BlurSigma, 1.0);
   nh.param("Calibrator_MeanGate", Calibrator_MeanGate, 10.0);
   nh.param("Calibrator_MinCornersForGrabbedImage", Calibrator_MinCornersForGrabbedImage, 20);
@@ -57,6 +57,6 @@ void FixParams::readFixParams()
   nh.param("GLWindowMenu_mgvnMenuTextOffset", GLWindowMenu_mgvnMenuTextOffset, 20);
   nh.param("InitLevel", InitLevel, 1);
   nh.param("parent_frame", parent_frame, std::string("world"));
-  nh.param("gui", gui, false);
+  nh.param("gui", gui, true);
 }
 ;

ptam/src/System.cc

@@ -91,9 +91,29 @@ void System::imageCallback(const sensor_msgs::ImageConstPtr & img)
 {
 //	static ros::Time t = img->header.stamp;
 
+  // image-stub which will contain bw image
+  cv::Mat imgBW;
+
+//  ROS_ASSERT(img->step == img->width);
 
-  ROS_ASSERT(img->encoding == sensor_msgs::image_encodings::MONO8 && img->step == img->width);
-
+  if(img->encoding == sensor_msgs::image_encodings::RGB8)
+  {
+      // wrap around color data, and convert to bw image.
+      cv::Mat imgColTmp = cv::Mat(img->height, img->width,CV_8UC3, (uchar*)img->data.data());
+      cv::cvtColor(imgColTmp,imgBW,CV_RGB2GRAY);
+  }
+  else if(img->encoding == sensor_msgs::image_encodings::MONO8)
+  {
+      // just wrap around data... does not copy any image data.
+      imgBW = cv::Mat(img->height, img->width,CV_8U, (uchar*)img->data.data());
+  }
+  else
+  {
+    // throw error
+    ROS_ASSERT(img->encoding == sensor_msgs::image_encodings::RGB8 || img->encoding == sensor_msgs::image_encodings::MONO8);
+  }
+
+
   VarParams *varParams = ParamsAccess::varParams;
 
   if(first_frame_){
@@ -121,7 +141,7 @@ void System::imageCallback(const sensor_msgs::ImageConstPtr & img)
 
 //  -------------------
   // TODO: avoid copy, but calling TrackFrame, with the ros image, because there is another copy inside TrackFrame
-  CVD::BasicImage<CVD::byte> img_tmp((CVD::byte *)&img->data[0], CVD::ImageRef(img->width, img->height));
+  CVD::BasicImage<CVD::byte> img_tmp(imgBW.data, CVD::ImageRef(img->width, img->height));
   CVD::copy(img_tmp, img_bw_);
 
   bool tracker_draw = false;