00001
00012 #ifndef HARDWARE_SENSOR_CAMERA_HPP_
00013 #define HARDWARE_SENSOR_CAMERA_HPP_
00014
00015 #include <image/Image.hpp>
00016 #include <kernel/threads.hpp>
00017
00018 #include <boost/thread.hpp>
00019 #include <boost/thread/thread.hpp>
00020 #include <boost/thread/mutex.hpp>
00021
00022 #include "rtslam/hardwareSensorAbstract.hpp"
00023 #include "rtslam/rawImage.hpp"
00024
00025
00026 namespace jafar {
00027 namespace rtslam {
00028 namespace hardware {
00029
00034 class HardwareSensorCamera: public HardwareSensorExteroAbstract
00035 {
00036 protected:
00037 class LoggableImage: public kernel::Loggable
00038 {
00039 private:
00040 std::string dump_path;
00041 rawimage_ptr_t image;
00042 int index;
00043 size_t id;
00044 std::fstream &fb;
00045
00046 public:
00047 LoggableImage(std::string const & dump_path, rawimage_ptr_t const & image, int index, size_t id, std::fstream &fb):
00048 dump_path(dump_path), image(image), index(index), id(id), fb(fb) {}
00049 virtual void log()
00050 {
00051 std::ostringstream oss; oss << dump_path << "/image_";
00052 if (id < 1000) oss << id << "_";
00053 oss << std::setw(7) << std::setfill('0') << index;
00054 image->img->save(oss.str() + std::string(".pgm"));
00055 std::fstream f; f.open((oss.str() + std::string(".time")).c_str(), std::ios_base::out);
00056 f << std::setprecision(19) << image->timestamp << std::endl; f.close();
00057 fb.write((char*)&(index), sizeof(int));
00058 fb.write((char*)&(image->timestamp), sizeof(double));
00059 }
00060 };
00061
00062 protected:
00063 std::vector<IplImage*> bufferImage;
00064 std::vector<rawimage_ptr_t> bufferSpecPtr;
00065 std::list<rawimage_ptr_t> bufferSave;
00066 unsigned index;
00067 int found_first;
00068
00069 double realFreq;
00070 double last_timestamp;
00071 std::string dump_path;
00072
00073 boost::thread *preloadTask_thread;
00074 virtual void preloadTask(void) = 0;
00075 void preloadTaskOffline(void);
00076 kernel::LoggerTask *loggerTask;
00077
00078 int cam_id;
00079 int filter_div, filter_mod;
00080
00081 void init(Mode mode, std::string dump_path, cv::Size imgSize);
00082 virtual void start();
00083 virtual void stop();
00084 virtual bool join(int timed_ms = -1);
00085
00086 public:
00087 HardwareSensorCamera(kernel::VariableCondition<int> *condition, int cam_id, cv::Size imgSize, std::string dump_path = ".");
00088 HardwareSensorCamera(kernel::VariableCondition<int> *condition, Mode mode, int cam_id, int bufferSize, kernel::LoggerTask *loggerTask);
00089
00090 virtual double getLastTimestamp() { boost::unique_lock<boost::mutex> l(mutex_data); return last_timestamp; }
00091 double getFreq() { return realFreq; }
00092 inline int id() const { return cam_id; }
00093 void setFilter(int div, int mod)
00094 { filter_div = div; filter_mod = mod; }
00095 };
00096
00097
00098 }}}
00099
00100 #endif
