[Jderobot-admin] jderobot-r1012 - in trunk/src/stable: components/basic_component components/cameraserver components/cameraview components/gazeboserver components/gazeboserver/pluginsGazebo/kinect components/introrob components/kinectViewer components/opencvdemo components/openniServer components/recorder components/replayer components/rgbdCalibrator components/rgbdManualCalibrator libs libs/geometry/math libs/geometry/progeo libs/parallelIce libs/visionlib libs/visionlib/colorspaces
rocapal en jderobot.org
rocapal en jderobot.org
Lun Oct 7 15:38:13 CEST 2013
Author: rocapal
Date: 2013-10-07 15:37:13 +0200 (Mon, 07 Oct 2013)
New Revision: 1012
Added:
trunk/src/stable/libs/visionlib/colorspaces/
Removed:
trunk/src/stable/libs/colorspaces/
Modified:
trunk/src/stable/components/basic_component/API.h
trunk/src/stable/components/basic_component/control.h
trunk/src/stable/components/basic_component/gui.h
trunk/src/stable/components/cameraserver/cameraserver.cpp
trunk/src/stable/components/cameraview/cameraview.cfg
trunk/src/stable/components/cameraview/cameraview.cpp
trunk/src/stable/components/cameraview/viewer.h
trunk/src/stable/components/gazeboserver/camera_dump.cc
trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h
trunk/src/stable/components/introrob/API.h
trunk/src/stable/components/introrob/control.h
trunk/src/stable/components/introrob/gui.h
trunk/src/stable/components/kinectViewer/kinectViewer.cfg
trunk/src/stable/components/kinectViewer/kinectViewergui.cpp
trunk/src/stable/components/kinectViewer/kinectViewergui.h
trunk/src/stable/components/opencvdemo/opencvdemo.cfg
trunk/src/stable/components/opencvdemo/opencvdemo.cpp
trunk/src/stable/components/opencvdemo/viewer.h
trunk/src/stable/components/openniServer/openniServer.cfg
trunk/src/stable/components/openniServer/openniServer.cpp
trunk/src/stable/components/recorder/poolWriteImages.h
trunk/src/stable/components/recorder/poolWriteLasers.h
trunk/src/stable/components/recorder/poolWritePointCloud.h
trunk/src/stable/components/recorder/recorder.cpp
trunk/src/stable/components/recorder/recordergui.h
trunk/src/stable/components/replayer/replayer.cpp
trunk/src/stable/components/rgbdCalibrator/calibration.cpp
trunk/src/stable/components/rgbdCalibrator/calibration.h
trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
trunk/src/stable/components/rgbdCalibrator/viewer.cpp
trunk/src/stable/components/rgbdCalibrator/viewer.h
trunk/src/stable/components/rgbdManualCalibrator/controller.h
trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp
trunk/src/stable/components/rgbdManualCalibrator/view.h
trunk/src/stable/libs/geometry/math/Segment3D.cpp
trunk/src/stable/libs/geometry/progeo/Progeo.cpp
trunk/src/stable/libs/parallelIce/cameraClient.h
trunk/src/stable/libs/visionlib/CMakeLists.txt
trunk/src/stable/libs/visionlib/colorspaces/CMakeLists.txt
trunk/src/stable/libs/visionlib/linesDetection.h
Log:
#56 changed colorspaces into visionlib
Modified: trunk/src/stable/components/basic_component/API.h
===================================================================
--- trunk/src/stable/components/basic_component/API.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/basic_component/API.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,7 +33,7 @@
#include <jderobot/laser.h>
#include <jderobot/encoders.h>
#include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <pthread.h>
Modified: trunk/src/stable/components/basic_component/control.h
===================================================================
--- trunk/src/stable/components/basic_component/control.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/basic_component/control.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,7 +33,7 @@
#include <jderobot/laser.h>
#include <jderobot/encoders.h>
#include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <pthread.h>
#include "API.h"
Modified: trunk/src/stable/components/basic_component/gui.h
===================================================================
--- trunk/src/stable/components/basic_component/gui.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/basic_component/gui.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -22,7 +22,7 @@
#ifndef BASIC_COMPONENT_GUI_H
#define BASIC_COMPONENT_GUI_H
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include<gtk-2.0/gtk/gtk.h>
#include<gtk-2.0/gdk/gdk.h>
#include<gtkmm-2.4/gtkmm.h>
@@ -77,4 +77,4 @@
}; //class
}//namespace
-#endif //BASIC_COMPONENT_GUI_H
\ No newline at end of file
+#endif //BASIC_COMPONENT_GUI_H
Modified: trunk/src/stable/components/cameraserver/cameraserver.cpp
===================================================================
--- trunk/src/stable/components/cameraserver/cameraserver.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraserver/cameraserver.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -26,7 +26,7 @@
#include <jderobot/camera.h>
#include <jderobot/image.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
//Opencv
Modified: trunk/src/stable/components/cameraview/cameraview.cfg
===================================================================
--- trunk/src/stable/components/cameraview/cameraview.cfg 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraview/cameraview.cfg 2013-10-07 13:37:13 UTC (rev 1012)
@@ -1 +1 @@
-Cameraview.Camera.Proxy=cameraB:default -h localhost -p 9999
+Cameraview.Camera.Proxy=cameraB:default -h localhost -p 9998
Modified: trunk/src/stable/components/cameraview/cameraview.cpp
===================================================================
--- trunk/src/stable/components/cameraview/cameraview.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraview/cameraview.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -23,7 +23,7 @@
#include <Ice/Ice.h>
#include <IceUtil/IceUtil.h>
#include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include "viewer.h"
Modified: trunk/src/stable/components/cameraview/viewer.h
===================================================================
--- trunk/src/stable/components/cameraview/viewer.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/cameraview/viewer.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -27,7 +27,7 @@
#include <IceUtil/Thread.h>
#include <IceUtil/Time.h>
#include <string>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
namespace cameraview{
Modified: trunk/src/stable/components/gazeboserver/camera_dump.cc
===================================================================
--- trunk/src/stable/components/gazeboserver/camera_dump.cc 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/gazeboserver/camera_dump.cc 2013-10-07 13:37:13 UTC (rev 1012)
@@ -13,7 +13,7 @@
#include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <iostream>
Modified: trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h
===================================================================
--- trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/gazeboserver/pluginsGazebo/kinect/kinectPlugin.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -35,7 +35,7 @@
#include <IceUtil/IceUtil.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <jderobot/pointcloud.h>
#include <jderobot/camera.h>
Modified: trunk/src/stable/components/introrob/API.h
===================================================================
--- trunk/src/stable/components/introrob/API.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/introrob/API.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -38,7 +38,7 @@
#include <jderobot/laser.h>
#include <jderobot/encoders.h>
#include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <jderobot/pose3dmotors.h>
#include <jderobot/pose3dencoders.h>
#include <pthread.h>
Modified: trunk/src/stable/components/introrob/control.h
===================================================================
--- trunk/src/stable/components/introrob/control.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/introrob/control.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,7 +33,7 @@
#include <jderobot/laser.h>
#include <jderobot/encoders.h>
#include <jderobot/ptencoders.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <pthread.h>
#include "API.h"
Modified: trunk/src/stable/components/introrob/gui.h
===================================================================
--- trunk/src/stable/components/introrob/gui.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/introrob/gui.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -37,7 +37,7 @@
#include <IceUtil/Time.h>
#include <jderobot/camera.h>
#include <pthread.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <gtkmm/drawingarea.h>
#include <gdkmm/pixbuf.h>
#include "API.h"
Modified: trunk/src/stable/components/kinectViewer/kinectViewer.cfg
===================================================================
--- trunk/src/stable/components/kinectViewer/kinectViewer.cfg 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/kinectViewer/kinectViewer.cfg 2013-10-07 13:37:13 UTC (rev 1012)
@@ -1,9 +1,9 @@
kinectViewer.CameraRGBActive=1
-kinectViewer.CameraRGB.Proxy=cameraA:tcp -h 127.0.0.1 -p 9999
+kinectViewer.CameraRGB.Proxy=cameraA:tcp -h 127.0.0.1 -p 9998
kinectViewer.CameraDEPTHActive=1
-kinectViewer.CameraDEPTH.Proxy=cameraB:tcp -h 127.0.0.1 -p 9999
+kinectViewer.CameraDEPTH.Proxy=cameraB:tcp -h 127.0.0.1 -p 9998
kinectViewer.pointCloudActive=1
-kinectViewer.pointCloud.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9999
+kinectViewer.pointCloud.Proxy=pointcloud1:tcp -h 127.0.0.1 -p 9998
kinectViewer.Pose3DMotorsActive=0
kinectViewer.Pose3DMotors.Proxy=Pose3DMotors1:tcp -h 127.0.0.1 -p 9998
kinectViewer.KinectLedsActive=0
Modified: trunk/src/stable/components/kinectViewer/kinectViewergui.cpp
===================================================================
--- trunk/src/stable/components/kinectViewer/kinectViewergui.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/kinectViewer/kinectViewergui.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -23,562 +23,567 @@
#include <jderobot/pointcloud.h>
namespace kinectViewer {
- kinectViewergui::kinectViewergui(bool rgb, bool depth,bool pointCloud , std::string path, std::string path_rgb, std::string path_ir, int width, int height, float cycle): gtkmain(0,0) {
+kinectViewergui::kinectViewergui(bool rgb, bool depth,bool pointCloud , std::string path, std::string path_rgb, std::string path_ir, int width, int height, float cycle): gtkmain(0,0) {
- /*Init OpenGL*/
- if(!Gtk::GL::init_check(NULL, NULL)) {
- std::cerr << "Couldn't initialize GL\n";
- std::exit(1);
- }
- this->cycle=cycle;
+ /*Init OpenGL*/
+ if(!Gtk::GL::init_check(NULL, NULL)) {
+ std::cerr << "Couldn't initialize GL\n";
+ std::exit(1);
+ }
+ this->cycle=cycle;
- if (rgb)
- cam_rgb_active=1;
- else
- cam_rgb_active=0;
- if (depth)
- cam_depth_active=1;
- else
- cam_depth_active=0;
- modesAvalables=0;
- if (pointCloud){
- if ((rgb)&&(depth)){
- reconstructMode=1;
- modesAvalables=2; //only one mode
- }
- else{
- reconstructMode=1;
- modesAvalables=1; //both modes
- }
- }
- else{
- if ((rgb)&&(depth)){
- reconstructMode=1;
- modesAvalables=1; //only point cloud mode
- }
- else{
- reconstructMode=0;
- modesAvalables=0; // no mode available
- }
- }
+ if (rgb)
+ cam_rgb_active=1;
+ else
+ cam_rgb_active=0;
+ if (depth)
+ cam_depth_active=1;
+ else
+ cam_depth_active=0;
+ modesAvalables=0;
+ if (pointCloud) {
+ if ((rgb)&&(depth)) {
+ reconstructMode=1;
+ modesAvalables=2; //only one mode
+ }
+ else {
+ reconstructMode=1;
+ modesAvalables=1; //both modes
+ }
+ }
+ else {
+ if ((rgb)&&(depth)) {
+ reconstructMode=1;
+ modesAvalables=1; //only point cloud mode
+ }
+ else {
+ reconstructMode=0;
+ modesAvalables=0; // no mode available
+ }
+ }
- reconstruct_depth_activate=false;
- lines_depth_active=false;
- lines_rgb_active=false;
- std::cout << "Loading glade\n";
- refXml = Gnome::Glade::Xml::create("./kinectViewergui.glade");
- cWidth=width;
- cHeight=height;
+ reconstruct_depth_activate=false;
+ lines_depth_active=false;
+ lines_rgb_active=false;
+ std::cout << "Loading glade\n";
+ refXml = Gnome::Glade::Xml::create("./kinectViewergui.glade");
+ cWidth=width;
+ cHeight=height;
-
- /*Get widgets*/
- refXml->get_widget("kinectViewer", mainwindow);
- refXml->get_widget("imageRGB", w_imageRGB);
- refXml->get_widget("imageDEPTH", w_imageDEPTH);
- refXml->get_widget("eventboxRGB", w_event_rgb);
- refXml->get_widget("eventboxDEPTH", w_event_depth);
- refXml->get_widget("toggle_reconstruct", w_reconstruct);
- refXml->get_widget("toggle_camera_pos", w_camera_pos);
- refXml->get_widget("toggle_lines_rgb", w_lines_rgb);
- refXml->get_widget("toggle_lines_depth", w_lines_depth);
- refXml->get_widget("toggle_depth", w_toggle_depth);
- refXml->get_widget("toggle_rgb", w_toggle_rgb);
- refXml->get_widget("view_controller",w_view_controller);
- refXml->get_widget("window_controller",w_window_controller);
- refXml->get_widget("vbox_reconstruct_selection",w_reconstruct_selection);
- refXml->get_widget("vbox_reconstruct_mode",w_reconstruct_mode);
- refXml->get_widget("radio_mode_pointcloud",w_radio_mode_pointcloud);
- refXml->get_widget("radio_mode_image",w_radio_mode_image);
- refXml->get_widget("radio_depth",w_radio_depth);
- refXml->get_widget("radio_rgb",w_radio_rgb);
- refXml->get_widget("button_clear_lines",w_button_clear_lines);
- refXml->get_widget("window_gl",w_window_gl);
- refXml->get_widget("tg_gl",w_tg_gl);
- refXml->get_widget("vbox_gl",w_vbox_gl);
-
- if (!cam_rgb_active){
- w_toggle_rgb->hide();
- }
- if (!cam_depth_active){
- w_toggle_depth->hide();
- }
-
- w_event_rgb->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_rgb));
- w_event_depth->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_depth));
- w_reconstruct->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_reconstruct_depth));
- w_camera_pos->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::add_camera_position));
- w_lines_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_rgb_toggled));
- w_lines_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_depth_toggled));
- w_tg_gl->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_tg_gl_toggled));
- w_view_controller->signal_activate().connect(sigc::mem_fun(this,&kinectViewergui::on_w_view_controller_activate));
- w_radio_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_depth_activate));
- w_radio_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_rgb_activate));
- w_radio_mode_pointcloud->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_pointcloud_activate));
- w_radio_mode_image->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_image_activate));
- w_button_clear_lines->signal_clicked().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_clear_lines));
- if (modesAvalables==0){
- w_reconstruct->hide();
- }
+ /*Get widgets*/
+ refXml->get_widget("kinectViewer", mainwindow);
+ refXml->get_widget("imageRGB", w_imageRGB);
+ refXml->get_widget("imageDEPTH", w_imageDEPTH);
+ refXml->get_widget("eventboxRGB", w_event_rgb);
+ refXml->get_widget("eventboxDEPTH", w_event_depth);
+ refXml->get_widget("toggle_reconstruct", w_reconstruct);
+ refXml->get_widget("toggle_camera_pos", w_camera_pos);
+ refXml->get_widget("toggle_lines_rgb", w_lines_rgb);
+ refXml->get_widget("toggle_lines_depth", w_lines_depth);
+ refXml->get_widget("toggle_depth", w_toggle_depth);
+ refXml->get_widget("toggle_rgb", w_toggle_rgb);
+ refXml->get_widget("view_controller",w_view_controller);
+ refXml->get_widget("window_controller",w_window_controller);
+ refXml->get_widget("vbox_reconstruct_selection",w_reconstruct_selection);
+ refXml->get_widget("vbox_reconstruct_mode",w_reconstruct_mode);
+ refXml->get_widget("radio_mode_pointcloud",w_radio_mode_pointcloud);
+ refXml->get_widget("radio_mode_image",w_radio_mode_image);
+ refXml->get_widget("radio_depth",w_radio_depth);
+ refXml->get_widget("radio_rgb",w_radio_rgb);
+ refXml->get_widget("button_clear_lines",w_button_clear_lines);
+ refXml->get_widget("window_gl",w_window_gl);
+ refXml->get_widget("tg_gl",w_tg_gl);
+ refXml->get_widget("vbox_gl",w_vbox_gl);
- // Mundo OpenGL
+ if (!cam_rgb_active) {
+ w_toggle_rgb->hide();
+ }
+ if (!cam_depth_active) {
+ w_toggle_depth->hide();
+ }
+
+ w_event_rgb->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_rgb));
+ w_event_depth->signal_button_press_event().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_event_depth));
+ w_reconstruct->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_reconstruct_depth));
+ w_camera_pos->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::add_camera_position));
+ w_lines_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_rgb_toggled));
+ w_lines_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_lines_depth_toggled));
+ w_tg_gl->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_tg_gl_toggled));
+ w_view_controller->signal_activate().connect(sigc::mem_fun(this,&kinectViewergui::on_w_view_controller_activate));
+ w_radio_depth->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_depth_activate));
+ w_radio_rgb->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_rgb_activate));
+ w_radio_mode_pointcloud->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_pointcloud_activate));
+ w_radio_mode_image->signal_toggled().connect(sigc::mem_fun(this,&kinectViewergui::on_w_radio_mode_image_activate));
+ w_button_clear_lines->signal_clicked().connect(sigc::mem_fun(this,&kinectViewergui::on_clicked_clear_lines));
+
+ if (modesAvalables==0) {
+ w_reconstruct->hide();
+ }
+
+ // Mundo OpenGL
refXml->get_widget_derived("gl_world",world);
- world->setCamerasResolution(width,height);
+ world->setCamerasResolution(width,height);
- std::cout << "Creating Progeos Virtual Cameras" << std::endl;
- mypro= new kinectViewer::myprogeo();
- mypro->load_cam((char*)path_rgb.c_str(),0,width, height);
+ std::cout << "Creating Progeos Virtual Cameras" << std::endl;
+ mypro= new kinectViewer::myprogeo();
+ mypro->load_cam((char*)path_rgb.c_str(),0,width, height);
- mypro->load_cam((char*)path_ir.c_str(),1,width, height);
- util = new kinectViewer::util3d(mypro);
+ mypro->load_cam((char*)path_ir.c_str(),1,width, height);
+ util = new kinectViewer::util3d(mypro);
- /*Show window. Note: Set window visibility to false in Glade, otherwise opengl won't work*/
- world->readFile(path);
- mainwindow->show();
+ /*Show window. Note: Set window visibility to false in Glade, otherwise opengl won't work*/
+ world->readFile(path);
+ mainwindow->show();
- }
+}
- kinectViewergui::~kinectViewergui() {
- //delete this->controller;
- }
+kinectViewergui::~kinectViewergui() {
+ //delete this->controller;
+}
-void
+void
kinectViewergui::updateAll( cv::Mat imageRGB, cv::Mat imageDEPTH, std::vector<jderobot::RGBPoint> cloud )
{
- std::cout << imageRGB.rows << std::endl;
+ std::cout << imageRGB.rows << std::endl;
- cv::Mat distance(imageRGB.rows, imageRGB.cols, CV_32FC1);
- cv::Mat colorDepth(imageDEPTH.size(),imageDEPTH.type());
- CvPoint pt1,pt2;
- if (w_toggle_rgb->get_active()){
- Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
- w_imageRGB->clear();
- /*si queremos pintar las lineas*/
- if (lines_rgb_active){
- util->draw_room(imageRGB,0, world->lines, world->numlines);
- }
- w_imageRGB->set(imgBuff);
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
- if (w_toggle_depth->get_active()||((reconstruct_depth_activate)&&(reconstructMode==0))){
-
- /*split channels to separate distance from image*/
- std::vector<cv::Mat> layers;
- cv::split(imageDEPTH, layers);
+ cv::Mat distance(imageRGB.rows, imageRGB.cols, CV_32FC1);
+ cv::Mat colorDepth(imageDEPTH.size(),imageDEPTH.type());
+ CvPoint pt1,pt2;
+ if (w_toggle_rgb->get_active()) {
+ Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
+ w_imageRGB->clear();
+ /*si queremos pintar las lineas*/
+ if (lines_rgb_active) {
+ util->draw_room(imageRGB,0, world->lines, world->numlines);
+ }
+ w_imageRGB->set(imgBuff);
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+ }
+ if (w_toggle_depth->get_active()||((reconstruct_depth_activate)&&(reconstructMode==0))) {
- cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
- /*cv::imshow("color", colorDepth);
- cv::waitKey(1);*/
+ /*split channels to separate distance from image*/
+ std::vector<cv::Mat> layers;
+ cv::split(imageDEPTH, layers);
- for (int x=0; x< layers[1].cols ; x++){
- for (int y=0; y<layers[1].rows; y++){
- distance.at<float>(y,x) = ((int)layers[1].at<unsigned char>(y,x)<<8)|(int)layers[2].at<unsigned char>(y,x); }
- }
+ cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
+ /*cv::imshow("color", colorDepth);
+ cv::waitKey(1);*/
- if (w_toggle_depth->get_active()){
- cv::Mat localDepth;
- Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,Gdk::COLORSPACE_RGB,false,8,colorDepth.cols,colorDepth.rows,colorDepth.step);
- w_imageDEPTH->clear();
- if (lines_depth_active){
- util->draw_room(colorDepth,1, world->lines, world->numlines);
- }
- w_imageDEPTH->set(imgBuff);
- }
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
- if (reconstruct_depth_activate){
- if (reconstructMode==0){
- add_depth_pointsImage(imageRGB, distance);
- }
- else
- add_depth_pointsCloud(cloud);
- }
- world->my_expose_event();
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
+ for (int x=0; x< layers[1].cols ; x++) {
+ for (int y=0; y<layers[1].rows; y++) {
+ distance.at<float>(y,x) = ((int)layers[1].at<unsigned char>(y,x)<<8)|(int)layers[2].at<unsigned char>(y,x);
+ }
+ }
-void
+ if (w_toggle_depth->get_active()) {
+ cv::Mat localDepth;
+ Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,Gdk::COLORSPACE_RGB,false,8,colorDepth.cols,colorDepth.rows,colorDepth.step);
+ w_imageDEPTH->clear();
+ if (lines_depth_active) {
+ util->draw_room(colorDepth,1, world->lines, world->numlines);
+ }
+ w_imageDEPTH->set(imgBuff);
+ }
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+ }
+ if (reconstruct_depth_activate) {
+ if (reconstructMode==0) {
+ add_depth_pointsImage(imageRGB, distance);
+ }
+ else
+ add_depth_pointsCloud(cloud);
+ }
+ world->my_expose_event();
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+}
+
+void
kinectViewergui::updateRGB( cv::Mat imageRGB)
{
- CvPoint pt1,pt2;
- if (w_toggle_rgb->get_active()){
- Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
- w_imageRGB->clear();
- /*si queremos pintar las lineas*/
- if (lines_rgb_active){
- util->draw_room(imageRGB,0, world->lines, world->numlines);
-
- }
- w_imageRGB->set(imgBuff);
- displayFrameRate();
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
- if (reconstruct_depth_activate){
- if (reconstructMode!=0){
- //add_depth_pointsCloud();
- }
- }
- world->my_expose_event();
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
+ CvPoint pt1,pt2;
+ if (w_toggle_rgb->get_active()) {
+ Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) imageRGB.data,Gdk::COLORSPACE_RGB,false,8,imageRGB.cols,imageRGB.rows,imageRGB.step);
+ w_imageRGB->clear();
+ /*si queremos pintar las lineas*/
+ if (lines_rgb_active) {
+ util->draw_room(imageRGB,0, world->lines, world->numlines);
-void
+ }
+ w_imageRGB->set(imgBuff);
+ displayFrameRate();
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+ }
+ if (reconstruct_depth_activate) {
+ if (reconstructMode!=0) {
+ //add_depth_pointsCloud();
+ }
+ }
+ world->my_expose_event();
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+}
+
+void
kinectViewergui::updateDEPTH(cv::Mat imageDEPTH )
{
- CvPoint pt1,pt2;
- if (w_toggle_depth->get_active()){
- Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) imageDEPTH.data,Gdk::COLORSPACE_RGB,false,8,imageDEPTH.cols,imageDEPTH.rows,imageDEPTH.step);
- w_imageDEPTH->clear();
- if (lines_depth_active){
- util->draw_room(imageDEPTH,1, world->lines, world->numlines);
-
- }
- w_imageDEPTH->set(imgBuff);
- displayFrameRate();
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
- if (reconstruct_depth_activate){
- if (reconstructMode!=0){
- //add_depth_pointsCloud();
- }
- }
- world->my_expose_event();
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
+ CvPoint pt1,pt2;
+ if (w_toggle_depth->get_active()) {
+ Glib::RefPtr<Gdk::Pixbuf> imgBuff = Gdk::Pixbuf::create_from_data((const guint8*) imageDEPTH.data,Gdk::COLORSPACE_RGB,false,8,imageDEPTH.cols,imageDEPTH.rows,imageDEPTH.step);
+ w_imageDEPTH->clear();
+ if (lines_depth_active) {
+ util->draw_room(imageDEPTH,1, world->lines, world->numlines);
-void
+ }
+ w_imageDEPTH->set(imgBuff);
+ displayFrameRate();
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+ }
+ if (reconstruct_depth_activate) {
+ if (reconstructMode!=0) {
+ //add_depth_pointsCloud();
+ }
+ }
+ world->my_expose_event();
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+}
+
+void
kinectViewergui::updatePointCloud(std::vector<jderobot::RGBPoint> cloud )
{
- displayFrameRate();
- while (gtkmain.events_pending())
- gtkmain.iteration();
- if (reconstruct_depth_activate){
- if (reconstructMode==1){
- add_depth_pointsCloud(cloud);
- }
- }
- world->my_expose_event();
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
+ displayFrameRate();
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+ if (reconstruct_depth_activate) {
+ if (reconstructMode==1) {
+ add_depth_pointsCloud(cloud);
+ }
+ }
+ world->my_expose_event();
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+}
- bool kinectViewergui::isClosed(){
- return false;
- }
+bool kinectViewergui::isClosed() {
+ return false;
+}
- bool kinectViewergui::isVisible() {
+bool kinectViewergui::isVisible() {
return mainwindow->is_visible();
- }
+}
void kinectViewergui::displayFrameRate()
{
- double diff;
- IceUtil::Time diffT;
+ double diff;
+ IceUtil::Time diffT;
- currentFrameTime = IceUtil::Time::now();
- diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
- if (diff < 1000.0)
- frameCount++;
- else{
- oldFrameTime = currentFrameTime;
- fps = frameCount*1000.0/diff;
- frameCount=0;
- // Display the frame rate
- std::stringstream fpsString;
- fpsString << "fps = " << int(fps);
- //fpslabel->set_label(fpsString.str());
- }
- }
+ currentFrameTime = IceUtil::Time::now();
+ diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
+ if (diff < 1000.0)
+ frameCount++;
+ else {
+ oldFrameTime = currentFrameTime;
+ fps = frameCount*1000.0/diff;
+ frameCount=0;
+ // Display the frame rate
+ std::stringstream fpsString;
+ fpsString << "fps = " << int(fps);
+ //fpslabel->set_label(fpsString.str());
+ }
+}
-bool kinectViewergui::on_clicked_event_rgb(GdkEventButton* event){
- int x,y;
- float xp,yp,zp,camx,camy,camz;
- float xu,yu,zu;
- float k;
-
- gdk_window_at_pointer(&x,&y);
- std::cout << x << ", " << y << std::endl;
- mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
- xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
- yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
- zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+bool kinectViewergui::on_clicked_event_rgb(GdkEventButton* event) {
+ int x,y;
+ float xp,yp,zp,camx,camy,camz;
+ float xu,yu,zu;
+ float k;
- k= 5000;
- world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
- return true;
+ gdk_window_at_pointer(&x,&y);
+ std::cout << x << ", " << y << std::endl;
+ mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
+ xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+ yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+ zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+
+ k= 5000;
+ world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
+ return true;
}
-bool kinectViewergui::on_clicked_event_depth(GdkEventButton* event){
- int x,y;
- float xp,yp,zp,camx,camy,camz;
- float xu,yu,zu;
- float k;
-
- gdk_window_at_pointer(&x,&y);
- mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
- xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
- yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
- zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+bool kinectViewergui::on_clicked_event_depth(GdkEventButton* event) {
+ int x,y;
+ float xp,yp,zp,camx,camy,camz;
+ float xu,yu,zu;
+ float k;
- k= 5000;
- world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
-
- world->add_line(k*xu + camx,k*yu + camy,k*zu + camz,camx,camy,camz);
- return true;
+ gdk_window_at_pointer(&x,&y);
+ mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
+ xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+ yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+ zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+
+ k= 5000;
+ world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
+
+ world->add_line(k*xu + camx,k*yu + camy,k*zu + camz,camx,camy,camz);
+ return true;
}
-void kinectViewergui::add_cameras_position(){
- int x,y;
- float xp,yp,zp,camx,camy,camz;
- float xu,yu,zu;
- float k;
-
- gdk_window_at_pointer(&x,&y);
- mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
- xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
- yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
- zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+void kinectViewergui::add_cameras_position() {
+ int x,y;
+ float xp,yp,zp,camx,camy,camz;
+ float xu,yu,zu;
+ float k;
- k= 500;
- world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
- world->add_line(1905,800,1240,camx,camy,camz);
+ gdk_window_at_pointer(&x,&y);
+ mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,0);
+ xu=(xp-camx)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+ yu=(yp-camy)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+ zu=(zp-camz)/sqrt((xp-camx)*(xp-camx) + (yp-camy)*(yp-camy) + (zp-camz)*(zp-camz));
+
+ k= 500;
+ world->add_line(camx+k*xu,camy+k*yu,camz+k*zu,camx,camy,camz);
+ world->add_line(1905,800,1240,camx,camy,camz);
}
-void
-kinectViewergui::on_reconstruct_depth(){
- if (w_reconstruct->get_active()){
- reconstruct_depth_activate=true;
- world->draw_kinect_points=true;
- w_reconstruct_selection->show();
- if (modesAvalables==2)
- w_reconstruct_mode->show();
- }
- else{
- reconstruct_depth_activate=false;
- world->draw_kinect_points=false;
- w_reconstruct_selection->hide();
- }
+void
+kinectViewergui::on_reconstruct_depth() {
+ if (w_reconstruct->get_active()) {
+ reconstruct_depth_activate=true;
+ world->draw_kinect_points=true;
+ w_reconstruct_selection->show();
+ if (modesAvalables==2)
+ w_reconstruct_mode->show();
+ }
+ else {
+ reconstruct_depth_activate=false;
+ world->draw_kinect_points=false;
+ w_reconstruct_selection->hide();
+ }
}
-void
-kinectViewergui::add_depth_pointsImage(cv::Mat imageRGB, cv::Mat distance){
- float d;
- //std::cout << "point image" << std::endl;
+void
+kinectViewergui::add_depth_pointsImage(cv::Mat imageRGB, cv::Mat distance) {
+ float d;
+ //std::cout << "point image" << std::endl;
- world->clear_points();
- //std::cout << "inicio reconstrucción" << std::endl;
- for (int xIm=0; xIm< cWidth; xIm++){
- for (int yIm=0; yIm<cHeight ; yIm++){
- d=distance.at<float>(yIm,xIm);
- if (d!=0){
- //std::cout << d << std::endl;
- //d=d*10;
- float xp,yp,zp,camx,camy,camz;
- float ux,uy,uz;
- float x,y;
- float k;
- float c1x, c1y, c1z;
- float fx,fy,fz;
- float fmod;
- float t;
- float Fx,Fy,Fz;
-
+ world->clear_points();
+ //std::cout << "inicio reconstrucción" << std::endl;
+ for (int xIm=0; xIm< cWidth; xIm++) {
+ for (int yIm=0; yIm<cHeight ; yIm++) {
+ d=distance.at<float>(yIm,xIm);
+ if (d!=0) {
+ //std::cout << d << std::endl;
+ //d=d*10;
+ float xp,yp,zp,camx,camy,camz;
+ float ux,uy,uz;
+ float x,y;
+ float k;
+ float c1x, c1y, c1z;
+ float fx,fy,fz;
+ float fmod;
+ float t;
+ float Fx,Fy,Fz;
+
+
+
+ mypro->mybackproject(xIm, yIm, &xp, &yp, &zp, &camx, &camy, &camz,0);
+
- mypro->mybackproject(xIm, yIm, &xp, &yp, &zp, &camx, &camy, &camz,0);
-
- //vector unitario
- float modulo;
-
- modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
- mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 0);
- fmod = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
- fx = (c1x - camx)*fmod;
- fy = (c1y - camy)*fmod;
- fz = (c1z - camz) * fmod;
- ux = (xp-camx)*modulo;
- uy = (yp-camy)*modulo;
- uz = (zp-camz)*modulo;
+ //vector unitario
+ float modulo;
- Fx= d*fx + camx;
- Fy= d*fy + camy;
- Fz= d*fz + camz;
+ modulo = sqrt(1/(((camx-xp)*(camx-xp))+((camy-yp)*(camy-yp))+((camz-zp)*(camz-zp))));
+ mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 0);
- /* calculamos el punto real */
- t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
-
+ fmod = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
+ fx = (c1x - camx)*fmod;
+ fy = (c1y - camy)*fmod;
+ fz = (c1z - camz) * fmod;
+ ux = (xp-camx)*modulo;
+ uy = (yp-camy)*modulo;
+ uz = (zp-camz)*modulo;
- /*world->points[i][0]=distance*ux+camx;
- world->points[i][1]=distance*uy+camy;
- world->points[i][2]=distance*uz+camz;*/
- /*std::cout << xp << "," << yp << "," << zp << "," << modulo << std::endl;
- std::cout << xp-camx << "," << yp-camy<< "," << zp-camz << std::endl;
- std::cout << ux << "," << uy<< "," << uz << std::endl;*/
- //k= (80-yp)/uy;
- //std::cout << "distancia" << distance << std::endl;
- //std::cout<< t*ux + camx << ", " << t*uy + camy << ", " << t*uz + camz << std::endl;
- world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,(int)imageRGB.data[3*(yIm*cWidth+xIm)],(int)imageRGB.data[3*(yIm*cWidth+xIm)+1],(int)imageRGB.data[3*(yIm*cWidth+xIm)+2]);
+ Fx= d*fx + camx;
+ Fy= d*fy + camy;
+ Fz= d*fz + camz;
- //world->add_line(distance*ux + camx,distance*uy+ camy,distance*uz + camz,camx,camy,camz);
- }
- }
- }
+ /* calculamos el punto real */
+ t = (-(fx*camx) + (fx*Fx) - (fy*camy) + (fy*Fy) - (fz*camz) + (fz*Fz))/((fx*ux) + (fy*uy) + (fz*uz));
- //std::cout << "fin reconstrucción" << std::endl;
+
+
+ /*world->points[i][0]=distance*ux+camx;
+ world->points[i][1]=distance*uy+camy;
+ world->points[i][2]=distance*uz+camz;*/
+ //std::cout << c1x << "," << c1y << "," << c1f << "," << std::endl;
+ /*std::cout << xp-camx << "," << yp-camy<< "," << zp-camz << std::endl;
+ std::cout << ux << "," << uy<< "," << uz << std::endl;*/
+ //k= (80-yp)/uy;
+ //std::cout << "distancia" << distance << std::endl;
+ //std::cout<< t*ux + camx << ", " << t*uy + camy << ", " << t*uz + camz << std::endl;
+ world->add_kinect_point(t*ux + camx,t*uy+ camy,t*uz + camz,(int)imageRGB.data[3*(yIm*cWidth+xIm)],(int)imageRGB.data[3*(yIm*cWidth+xIm)+1],(int)imageRGB.data[3*(yIm*cWidth+xIm)+2]);
+
+ //world->add_line(distance*ux + camx,distance*uy+ camy,distance*uz + camz,camx,camy,camz);
+ }
+ }
+ }
+
+ //std::cout << "fin reconstrucción" << std::endl;
}
-void
-kinectViewergui::add_depth_pointsCloud(std::vector<jderobot::RGBPoint> cloud){
- world->clear_points();
- for (std::vector<jderobot::RGBPoint>::iterator it = cloud.begin(); it != cloud.end(); ++it){
- world->add_kinect_point(it->x,it->y,it->z,(int)it->r,(int)it->g,(int)it->b);
- }
+void
+kinectViewergui::add_depth_pointsCloud(std::vector<jderobot::RGBPoint> cloud) {
+ world->clear_points();
+ for (std::vector<jderobot::RGBPoint>::iterator it = cloud.begin(); it != cloud.end(); ++it) {
+ world->add_kinect_point(it->x,it->y,it->z,(int)it->r,(int)it->g,(int)it->b);
+ }
}
-void
-kinectViewergui::add_camera_position(){
- float c1x, c1y, c1z, c2x, c2y, c2z, c3x, c3y, c3z, c4x, c4y,c4z;
- float camx, camy, camz;
- float w,h;
- float modulo,distance;
-
+void
+kinectViewergui::add_camera_position() {
+ float c1x, c1y, c1z, c2x, c2y, c2z, c3x, c3y, c3z, c4x, c4y,c4z;
+ float camx, camy, camz;
+ float w,h;
+ float modulo,distance;
- if (w_camera_pos->get_active()){
- distance=300;
- mypro->mygetcamerasize(&w,&h,1);
- mypro->mybackproject(0,0,&c1x,&c1y,&c1z,&camx, &camy, &camz,1);
- mypro->mybackproject(0,cWidth,&c2x,&c2y,&c2z,&camx, &camy, &camz,1);
- mypro->mybackproject(cHeight,0,&c3x,&c3y,&c3z,&camx, &camy, &camz,1);
- mypro->mybackproject(cHeight,cWidth,&c4x,&c4y,&c4z,&camx, &camy, &camz,1);
-
- modulo = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
- c1x = (c1x-camx)*modulo;
- c1y = (c1y-camy)*modulo;
- c1z = (c1z-camz)*modulo;
-
- modulo = sqrt(1/(((camx-c2x)*(camx-c2x))+((camy-c2y)*(camy-c2y))+((camz-c2z)*(camz-c2z))));
- c2x = (c2x-camx)*modulo;
- c2y = (c2y-camy)*modulo;
- c2z = (c2z-camz)*modulo;
-
- modulo = sqrt(1/(((camx-c3x)*(camx-c3x))+((camy-c3y)*(camy-c3y))+((camz-c3z)*(camz-c3z))));
- c3x = (c3x-camx)*modulo;
- c3y = (c3y-camy)*modulo;
- c3z = (c3z-camz)*modulo;
-
- modulo = sqrt(1/(((camx-c4x)*(camx-c4x))+((camy-c4y)*(camy-c4y))+((camz-c4z)*(camz-c4z))));
- c4x = (c4x-camx)*modulo;
- c4y = (c4y-camy)*modulo;
- c4z = (c4z-camz)*modulo;
-
-
-
- world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
- world->add_camera_line(distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz,camx,camy,camz);
- world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,camx,camy,camz);
- world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,camx,camy,camz);
- world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
- world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
- world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz);
- world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz);
-
- mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 1);
- modulo = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
- c1x = (c1x-camx)*modulo;
- c1y = (c1y-camy)*modulo;
- c1z = (c1z-camz)*modulo;
- distance=distance*3;
- world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
- }
- else{
- world->clear_camera_lines();
- }
+ if (w_camera_pos->get_active()) {
+ distance=300;
+ mypro->mygetcamerasize(&w,&h,1);
+ mypro->mybackproject(0,0,&c1x,&c1y,&c1z,&camx, &camy, &camz,1);
+ mypro->mybackproject(0,cWidth,&c2x,&c2y,&c2z,&camx, &camy, &camz,1);
+ mypro->mybackproject(cHeight,0,&c3x,&c3y,&c3z,&camx, &camy, &camz,1);
+ mypro->mybackproject(cHeight,cWidth,&c4x,&c4y,&c4z,&camx, &camy, &camz,1);
- //mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
+ modulo = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
+ c1x = (c1x-camx)*modulo;
+ c1y = (c1y-camy)*modulo;
+ c1z = (c1z-camz)*modulo;
+
+ modulo = sqrt(1/(((camx-c2x)*(camx-c2x))+((camy-c2y)*(camy-c2y))+((camz-c2z)*(camz-c2z))));
+ c2x = (c2x-camx)*modulo;
+ c2y = (c2y-camy)*modulo;
+ c2z = (c2z-camz)*modulo;
+
+ modulo = sqrt(1/(((camx-c3x)*(camx-c3x))+((camy-c3y)*(camy-c3y))+((camz-c3z)*(camz-c3z))));
+ c3x = (c3x-camx)*modulo;
+ c3y = (c3y-camy)*modulo;
+ c3z = (c3z-camz)*modulo;
+
+ modulo = sqrt(1/(((camx-c4x)*(camx-c4x))+((camy-c4y)*(camy-c4y))+((camz-c4z)*(camz-c4z))));
+ c4x = (c4x-camx)*modulo;
+ c4y = (c4y-camy)*modulo;
+ c4z = (c4z-camz)*modulo;
+
+
+
+ world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
+ world->add_camera_line(distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz,camx,camy,camz);
+ world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,camx,camy,camz);
+ world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,camx,camy,camz);
+ world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
+ world->add_camera_line(distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz,distance*c2x + camx,distance*c2y+ camy,distance*c2z + camz);
+ world->add_camera_line(distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz,distance*c4x + camx,distance*c4y+ camy,distance*c4z + camz);
+ world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,distance*c3x + camx,distance*c3y+ camy,distance*c3z + camz);
+
+ mypro->mygetcamerafoa(&c1x, &c1y, &c1z, 1);
+ modulo = sqrt(1/(((camx-c1x)*(camx-c1x))+((camy-c1y)*(camy-c1y))+((camz-c1z)*(camz-c1z))));
+ c1x = (c1x-camx)*modulo;
+ c1y = (c1y-camy)*modulo;
+ c1z = (c1z-camz)*modulo;
+ distance=distance*3;
+ world->add_camera_line(distance*c1x + camx,distance*c1y+ camy,distance*c1z + camz,camx,camy,camz);
+ }
+ else {
+ world->clear_camera_lines();
+ }
+
+
+ //mypro->mybackproject(x, y, &xp, &yp, &zp, &camx, &camy, &camz,1);
}
-void
-kinectViewergui::on_w_lines_rgb_toggled(){
- if (w_lines_rgb->get_active()){
- lines_rgb_active=true;
- }
- else
- lines_rgb_active=false;
+void
+kinectViewergui::on_w_lines_rgb_toggled() {
+ if (w_lines_rgb->get_active()) {
+ lines_rgb_active=true;
+ }
+ else
+ lines_rgb_active=false;
}
-void
-kinectViewergui::on_w_lines_depth_toggled(){
- if (w_lines_depth->get_active()){
- lines_depth_active=true;
- }
- else
- lines_depth_active=false;
+void
+kinectViewergui::on_w_lines_depth_toggled() {
+ if (w_lines_depth->get_active()) {
+ lines_depth_active=true;
+ }
+ else
+ lines_depth_active=false;
}
void
-kinectViewergui::on_w_view_controller_activate(){
- w_window_controller->show();
+kinectViewergui::on_w_view_controller_activate() {
+ w_window_controller->show();
}
void
-kinectViewergui::on_w_radio_depth_activate(){
- if (w_radio_depth->get_active())
- world->draw_kinect_with_color=false;
+kinectViewergui::on_w_radio_depth_activate() {
+ if (w_radio_depth->get_active())
+ world->draw_kinect_with_color=false;
}
void
-kinectViewergui::on_w_radio_mode_pointcloud_activate(){
- if (w_radio_mode_pointcloud->get_active())
- reconstructMode=1;
+kinectViewergui::on_w_radio_mode_pointcloud_activate() {
+ if (w_radio_mode_pointcloud->get_active())
+ reconstructMode=1;
}
void
-kinectViewergui::on_w_radio_mode_image_activate(){
- if (w_radio_mode_image->get_active())
- reconstructMode=0;
+kinectViewergui::on_w_radio_mode_image_activate() {
+ if (w_radio_mode_image->get_active())
+ reconstructMode=0;
}
void
-kinectViewergui::on_w_radio_rgb_activate(){
- if (w_radio_rgb->get_active())
- world->draw_kinect_with_color=true;
+kinectViewergui::on_w_radio_rgb_activate() {
+ if (w_radio_rgb->get_active())
+ world->draw_kinect_with_color=true;
}
void
-kinectViewergui::on_clicked_clear_lines(){
- world->clearExtraLines();
+kinectViewergui::on_clicked_clear_lines() {
+ world->clearExtraLines();
}
-void
-kinectViewergui::on_w_tg_gl_toggled(){
- if (w_tg_gl->get_active()){
- w_window_gl->show();
- w_vbox_gl->show();
- }
- else{
- w_window_gl->hide();
- w_vbox_gl->hide();
- }
+void
+kinectViewergui::on_w_tg_gl_toggled() {
+ if (w_tg_gl->get_active()) {
+ w_window_gl->show();
+ w_vbox_gl->show();
+ }
+ else {
+ w_window_gl->hide();
+ w_vbox_gl->hide();
+ }
}
float
-kinectViewergui::getCycle(){
- return this->cycle;
+kinectViewergui::getCycle() {
+ return this->cycle;
}
} // namespace
Modified: trunk/src/stable/components/kinectViewer/kinectViewergui.h
===================================================================
--- trunk/src/stable/components/kinectViewer/kinectViewergui.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/kinectViewer/kinectViewergui.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -27,7 +27,7 @@
#include <gtkmm.h>
#include <libglademm.h>
#include "drawarea.h"
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <jderobot/camera.h>
#include <IceUtil/Thread.h>
#include <IceUtil/Time.h>
Modified: trunk/src/stable/components/opencvdemo/opencvdemo.cfg
===================================================================
--- trunk/src/stable/components/opencvdemo/opencvdemo.cfg 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/opencvdemo/opencvdemo.cfg 2013-10-07 13:37:13 UTC (rev 1012)
@@ -1 +1 @@
-Opencvdemo.Camera.Proxy=cameraA:tcp -h 127.0.0.1 -p 9999
+Opencvdemo.Camera.Proxy=cameraA:tcp -h 127.0.0.1 -p 9998
Modified: trunk/src/stable/components/opencvdemo/opencvdemo.cpp
===================================================================
--- trunk/src/stable/components/opencvdemo/opencvdemo.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/opencvdemo/opencvdemo.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -22,7 +22,7 @@
#include <Ice/Ice.h>
#include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include "viewer.h"
int main(int argc, char** argv){
Modified: trunk/src/stable/components/opencvdemo/viewer.h
===================================================================
--- trunk/src/stable/components/opencvdemo/viewer.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/opencvdemo/viewer.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -25,7 +25,7 @@
#include <gtkmm.h>
#include <libglademm.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#define DEGTORAD (3.14159264 / 180.0)
Modified: trunk/src/stable/components/openniServer/openniServer.cfg
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cfg 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/openniServer/openniServer.cfg 2013-10-07 13:37:13 UTC (rev 1012)
@@ -20,14 +20,14 @@
openniServer.CameraRGB.Format=RGB8
openniServer.CameraRGB.fps=10
openniServer.CameraRGB.PlayerDetection=0
-openniServer.CameraRGB.Mirror=1
+openniServer.CameraRGB.Mirror=0
#openniServer.calibration=./config/camRGB
openniServer.CameraDEPTH.Name=cameraB
openniServer.CameraDEPTH.Format=RGB8
openniServer.CameraDEPTH.fps=10
openniServer.CameraDEPTH.PlayerDetection=0
-openniServer.CameraDEPTH.Mirror=1
+openniServer.CameraDEPTH.Mirror=0
Modified: trunk/src/stable/components/openniServer/openniServer.cpp
===================================================================
--- trunk/src/stable/components/openniServer/openniServer.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/openniServer/openniServer.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -29,7 +29,7 @@
#include <jderobot/camera.h>
#include <jderobot/pose3dmotors.h>
#include <jderobot/pointcloud.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <tr1/memory>
#include <list>
#include <sstream>
Modified: trunk/src/stable/components/recorder/poolWriteImages.h
===================================================================
--- trunk/src/stable/components/recorder/poolWriteImages.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/poolWriteImages.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -30,7 +30,7 @@
#include <stdio.h>
#include <time.h>
#include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <fstream>
Modified: trunk/src/stable/components/recorder/poolWriteLasers.h
===================================================================
--- trunk/src/stable/components/recorder/poolWriteLasers.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/poolWriteLasers.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,7 +28,7 @@
#include <stdio.h>
#include <time.h>
#include <jderobot/laser.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <fstream>
Modified: trunk/src/stable/components/recorder/poolWritePointCloud.h
===================================================================
--- trunk/src/stable/components/recorder/poolWritePointCloud.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/poolWritePointCloud.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,7 +28,7 @@
#include <stdio.h>
#include <time.h>
#include <jderobot/pointcloud.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <fstream>
namespace recorder {
Modified: trunk/src/stable/components/recorder/recorder.cpp
===================================================================
--- trunk/src/stable/components/recorder/recorder.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/recorder.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -13,7 +13,7 @@
#include <jderobot/pose3dmotors.h>
#include <jderobot/pose3dencoders.h>
#include <jderobot/pointcloud.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
Modified: trunk/src/stable/components/recorder/recordergui.h
===================================================================
--- trunk/src/stable/components/recorder/recordergui.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/recorder/recordergui.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -29,7 +29,7 @@
#include <libglademm.h>
#include <IceUtil/Thread.h>
#include <IceUtil/Time.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
namespace recorder {
class recordergui {
Modified: trunk/src/stable/components/replayer/replayer.cpp
===================================================================
--- trunk/src/stable/components/replayer/replayer.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/replayer/replayer.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -34,7 +34,7 @@
#include <cv.h>
#include <highgui.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <jderobot/camera.h>
#include <jderobot/pointcloud.h>
#include <jderobot/pose3dencoders.h>
Modified: trunk/src/stable/components/rgbdCalibrator/calibration.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -21,460 +21,575 @@
#include <calibration.h>
#include <iostream>
-#include "../../libs/geometry/progeo/Progeo.h"
#include "../../libs/geometry/math/Point3D.h"
#include "../../libs/geometry/math/Segment3D.h"
+#include <../../libs/progeo/progeo.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit.h>
+
using namespace cv;
namespace rgbdCalibrator{
- // This source code is a modification of camera_calibration.cpp
- // You can see in OpenCV samples:
- // - samples/cpp/tutorial_code/calib3d/camera_calibration/
+// This source code is a modification of camera_calibration.cpp
+// You can see in OpenCV samples:
+// - samples/cpp/tutorial_code/calib3d/camera_calibration/
- Calibration::Calibration()
- {
- initPatternPoints();
- }
+Calibration::Calibration(): mProgeo(NULL)
+{
+ initPatternPoints();
- Calibration::~Calibration()
- {}
+}
- // Intrinsics
- bool Calibration::runCalibrationAndSave(Size &boardSize,
- float squareSize,
- int flag,
- Size imageSize,
- Mat& cameraMatrix,
- Mat& distCoeffs,
- vector<vector<Point2f> > imagePoints )
- {
- vector<Mat> rvecs, tvecs;
- vector<float> reprojErrs;
- double totalAvgErr = 0;
-
- bool ok = runCalibration (boardSize, squareSize, flag, imageSize,
- cameraMatrix, distCoeffs, imagePoints,
- rvecs, tvecs, reprojErrs, totalAvgErr);
+Calibration::~Calibration()
+{
+ delete(mProgeo);
+}
- std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
- << ". avg re projection error = " << totalAvgErr ;
+// Intrinsics
+bool Calibration::runCalibrationAndSave(Size &boardSize,
+ float squareSize,
+ int flag,
+ Size imageSize,
+ Mat& cameraMatrix,
+ Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints )
+{
+ vector<Mat> rvecs, tvecs;
+ vector<float> reprojErrs;
+ double totalAvgErr = 0;
- mKMatrix = Mat(cameraMatrix);
+ bool ok = runCalibration (boardSize, squareSize, flag, imageSize,
+ cameraMatrix, distCoeffs, imagePoints,
+ rvecs, tvecs, reprojErrs, totalAvgErr);
+ std::cout << (ok ? "Calibration succeeded" : "Calibration failed")
+ << ". avg re projection error = " << totalAvgErr ;
- return ok;
- }
-
- bool Calibration::runCalibration(Size &boardSize,
- float squareSize,
- int flag,
- Size& imageSize,
- Mat& cameraMatrix,
- Mat& distCoeffs,
- vector<vector<Point2f> > imagePoints,
- vector<Mat>& rvecs,
- vector<Mat>& tvecs,
- vector<float>& reprojErrs,
- double& totalAvgErr)
- {
+ mKMatrix = Mat(cameraMatrix);
- Pattern calibrationPattern = CHESSBOARD;
+ initProgeo();
- cameraMatrix = Mat::eye(3, 3, CV_64F);
- if( flag & CV_CALIB_FIX_ASPECT_RATIO )
- cameraMatrix.at<double>(0,0) = 1.0;
+ return ok;
+}
- distCoeffs = Mat::zeros(8, 1, CV_64F);
-
- vector<vector<Point3f> > objectPoints(1);
- calcBoardCornerPositions(boardSize, squareSize, objectPoints[0], calibrationPattern);
-
- objectPoints.resize(imagePoints.size(),objectPoints[0]);
-
- //Find intrinsic and extrinsic camera parameters
- double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
- distCoeffs, rvecs, tvecs, flag|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
-
- std::cout << "Re-projection error reported by calibrateCamera: "<< rms << std::endl;
-
- bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
-
- totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
- rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
-
- return ok;
+bool Calibration::runCalibration(Size &boardSize,
+ float squareSize,
+ int flag,
+ Size& imageSize,
+ Mat& cameraMatrix,
+ Mat& distCoeffs,
+ vector<vector<Point2f> > imagePoints,
+ vector<Mat>& rvecs,
+ vector<Mat>& tvecs,
+ vector<float>& reprojErrs,
+ double& totalAvgErr)
+{
+
+ Pattern calibrationPattern = CHESSBOARD;
+
+ cameraMatrix = Mat::eye(3, 3, CV_64F);
+ if( flag & CV_CALIB_FIX_ASPECT_RATIO )
+ cameraMatrix.at<double>(0,0) = 1.0;
+
+ distCoeffs = Mat::zeros(8, 1, CV_64F);
+
+ vector<vector<Point3f> > objectPoints(1);
+ calcBoardCornerPositions(boardSize, squareSize, objectPoints[0], calibrationPattern);
+
+ objectPoints.resize(imagePoints.size(),objectPoints[0]);
+
+ //Find intrinsic and extrinsic camera parameters
+ double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix,
+ distCoeffs, rvecs, tvecs, flag|CV_CALIB_FIX_K4|CV_CALIB_FIX_K5);
+
+ std::cout << "Re-projection error reported by calibrateCamera: "<< rms << std::endl;
+
+ bool ok = checkRange(cameraMatrix) && checkRange(distCoeffs);
+
+ totalAvgErr = computeReprojectionErrors(objectPoints, imagePoints,
+ rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
+
+ return ok;
}
- void Calibration::calcBoardCornerPositions(Size boardSize,
- float squareSize,
- vector<Point3f>& corners,
- Pattern patternType /*= Settings::CHESSBOARD*/)
- {
- corners.clear();
-
- switch(patternType)
- {
- case CHESSBOARD:
- case CIRCLES_GRID:
- for( int i = 0; i < boardSize.height; ++i )
- for( int j = 0; j < boardSize.width; ++j )
- corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0));
- break;
-
- case ASYMMETRIC_CIRCLES_GRID:
- for( int i = 0; i < boardSize.height; i++ )
- for( int j = 0; j < boardSize.width; j++ )
- corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
- break;
- default:
- break;
- }
- }
+void Calibration::calcBoardCornerPositions(Size boardSize,
+ float squareSize,
+ vector<Point3f>& corners,
+ Pattern patternType /*= Settings::CHESSBOARD*/)
+{
+ corners.clear();
+ switch(patternType)
+ {
+ case CHESSBOARD:
+ case CIRCLES_GRID:
+ for( int i = 0; i < boardSize.height; ++i )
+ for( int j = 0; j < boardSize.width; ++j )
+ corners.push_back(Point3f(float( j*squareSize ), float( i*squareSize ), 0));
+ break;
+ case ASYMMETRIC_CIRCLES_GRID:
+ for( int i = 0; i < boardSize.height; i++ )
+ for( int j = 0; j < boardSize.width; j++ )
+ corners.push_back(Point3f(float((2*j + i % 2)*squareSize), float(i*squareSize), 0));
+ break;
+ default:
+ break;
+ }
+}
- double Calibration::computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
- const vector<vector<Point2f> >& imagePoints,
- const vector<Mat>& rvecs,
- const vector<Mat>& tvecs,
- const Mat& cameraMatrix,
- const Mat& distCoeffs,
- vector<float>& perViewErrors)
- {
- vector<Point2f> imagePoints2;
- int i, totalPoints = 0;
- double totalErr = 0, err;
- perViewErrors.resize(objectPoints.size());
-
- for( i = 0; i < (int)objectPoints.size(); ++i )
- {
- projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
- distCoeffs, imagePoints2);
- err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
-
- int n = (int)objectPoints[i].size();
- perViewErrors[i] = (float) std::sqrt(err*err/n);
- totalErr += err*err;
- totalPoints += n;
- }
-
- return std::sqrt(totalErr/totalPoints);
- }
+double Calibration::computeReprojectionErrors( const vector<vector<Point3f> >& objectPoints,
+ const vector<vector<Point2f> >& imagePoints,
+ const vector<Mat>& rvecs,
+ const vector<Mat>& tvecs,
+ const Mat& cameraMatrix,
+ const Mat& distCoeffs,
+ vector<float>& perViewErrors)
+{
+ vector<Point2f> imagePoints2;
+ int i, totalPoints = 0;
+ double totalErr = 0, err;
+ perViewErrors.resize(objectPoints.size());
- void
- Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
- const colorspaces::Image depthData,
- Eigen::Vector4d& res3D)
- {
+ for( i = 0; i < (int)objectPoints.size(); ++i )
+ {
+ projectPoints( Mat(objectPoints[i]), rvecs[i], tvecs[i], cameraMatrix,
+ distCoeffs, imagePoints2);
+ err = norm(Mat(imagePoints[i]), Mat(imagePoints2), CV_L2);
+ int n = (int)objectPoints[i].size();
+ perViewErrors[i] = (float) std::sqrt(err*err/n);
+ totalErr += err*err;
+ totalPoints += n;
+ }
- Eigen::Vector4d posCamera;
- posCamera(0) = double(0.0);
- posCamera(1) = double(0.0);
- posCamera(2) = double(0.0);
+ return std::sqrt(totalErr/totalPoints);
+}
+void progeo_old_test ()
+{
+ TPinHoleCamera myCamA;
+ xmlReader(&myCamA, "./cameraB.xml");
+ display_camerainfo(myCamA);
- Eigen::Matrix3d K;
- K(0,0) = mKMatrix.at<double>(0,0);
- K(0,1) = mKMatrix.at<double>(0,1);
- K(0,2) = mKMatrix.at<double>(0,2);
+ update_camera_matrix(&myCamA);
+ display_camerainfo(myCamA);
- K(1,0) = mKMatrix.at<double>(1,0);
- K(1,1) = mKMatrix.at<double>(1,1);
- K(1,2) = mKMatrix.at<double>(1,2);
+ HPoint2D pixel;
+ pixel.x =224.;
+ pixel.y =18.;
+ pixel.h = 1.;
- K(2,0) = mKMatrix.at<double>(2,0);
- K(2,1) = mKMatrix.at<double>(2,1);
- K(2,2) = mKMatrix.at<double>(2,2);
+ std::cout << pixel.x << " , " << pixel.y << std::endl;
- Eigen::Matrix4d RT;
- RT(0,0) = double(1.);
- RT(0,1) = double(0.);
- RT(0,2) = double(0.);
- RT(0,3) = double(0.);
+ HPoint3D p3D;
+ backproject(&p3D, pixel, myCamA);
- RT(1,0) = double(0.);
- RT(1,1) = double(1.);
- RT(1,2) = double(0.);
- RT(1,3) = double(0.);
+ std::cout << p3D.X << " , " << p3D.Y << " , " << p3D.Z << std::endl;
- RT(2,0) = double(0.);
- RT(2,1) = double(0.);
- RT(2,2) = double(1.);
- RT(2,3) = double(0.);
+}
- RT(3,0) = double(0.);
- RT(3,1) = double(0.);
- RT(3,2) = double(0.);
- RT(3,3) = double(1.);
- float depth = (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+2];
+void Calibration::initProgeo()
+{
- Progeo::Progeo* progeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
- //progeo->display_camerainfo();
+ Eigen::Vector4d posCamera;
+ posCamera(0) = double(0.0);
+ posCamera(1) = double(0.0);
+ posCamera(2) = double(0.0);
+ posCamera(3) = double(1.0);
- //std::cout << "Pixel (opticas): " << pixel << std::endl;
-
- // Optical coordinates
- Eigen::Vector3d graphic;
- graphic(0) = pixel(1);
- graphic(1) = 240 - 1 - pixel(0);
- graphic(2) = 1.0;
- //std::cout << "Graphic (progeo): " << graphic << std::endl;
+ Eigen::Matrix3d K;
+ K(0,0) = mKMatrix.at<double>(0,0);
+ K(0,1) = mKMatrix.at<double>(0,1);
+ K(0,2) = mKMatrix.at<double>(0,2);
- Eigen::Vector4d p3D;
- progeo->backproject(graphic, p3D);
+ K(1,0) = mKMatrix.at<double>(1,0);
+ K(1,1) = mKMatrix.at<double>(1,1);
+ K(1,2) = mKMatrix.at<double>(1,2);
- //std::cout << "P3D: " << p3D << std::endl;
+ K(2,0) = mKMatrix.at<double>(2,0);
+ K(2,1) = mKMatrix.at<double>(2,1);
+ K(2,2) = mKMatrix.at<double>(2,2);
- Point3D *pStart = new Point3D(0.0,0.0,0.0);
- Point3D *pEnd = new Point3D(p3D);
+ Eigen::Matrix4d RT;
+ RT(0,0) = double(1.);
+ RT(0,1) = double(0.);
+ RT(0,2) = double(0.);
+ RT(0,3) = double(0.);
- Segment3D *segment = new Segment3D(*pStart,*pEnd);
+ RT(1,0) = double(0.);
+ RT(1,1) = double(1.);
+ RT(1,2) = double(0.);
+ RT(1,3) = double(0.);
- //std::cout << "Depth: " << depth << std::endl;
+ RT(2,0) = double(0.);
+ RT(2,1) = double(0.);
+ RT(2,2) = double(1.);
+ RT(2,3) = double(0.);
- Point3D *nP3D = segment->getPointByZ(depth);
+ RT(3,0) = double(0.);
+ RT(3,1) = double(0.);
+ RT(3,2) = double(0.);
+ RT(3,3) = double(1.);
- res3D = nP3D->getPoint();
+ mProgeo = new Progeo::Progeo(posCamera, K, RT, 320, 240);
+ mProgeo->updateRTMatrix();
+ mProgeo->displayCameraInfo();
+ mProgeo->saveToFile("./CameraA.xml");
- //std::cout << res3D << std::endl;
+ Eigen::Vector3d pixel(224.0, 18., 1.0);
+ Eigen::Vector4d p3d;
+ mProgeo->backproject(pixel,p3d);
+ std::cout << "Pixel: " << pixel << std::endl;
+ std::cout << "P3d: " << p3d << std::endl;
- /*
- std::cout << "-------------" << std::endl;
- p3D(0) = 0.;
- p3D(1) = 0.;
- p3D(2) = 900.;
- p3D(3)= 1.;
- std::cout << p3D << std::endl;
- progeo->project(p3D, optical);
- std::cout << optical << std::endl;
+ progeo_old_test();
+}
- std::cout << "-------------" << std::endl;
- optical(0) = 0.;
- optical(1) = 0.;
- optical(2) = 1.;
- std::cout << optical << std::endl;
- progeo->backproject(optical, p3D);
- std::cout << p3D << std::endl;
- */
+void
+Calibration::BackProjectWithDepth (const Eigen::Vector3d pixel,
+ const colorspaces::Image depthData,
+ Eigen::Vector4d& res3D)
+{
- delete(segment);
- delete(pStart);
- delete(pEnd);
- delete(nP3D);
- delete(progeo);
+ float depth = (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+2];
- }
- void Calibration::initPatternPoints()
- {
- // Down line, right to left
- mPatternPoints.push_back(Eigen::Vector4d (120.,0.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (60.0,0.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,0.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,0.,60.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,0.,120.,1.));
+ //std::cout << "Pixel (opticas): " << pixel << std::endl;
- // Up line, right to left
- mPatternPoints.push_back(Eigen::Vector4d (120.,120.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (60.,120.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,120.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,120.,60.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,120.,120.,1.));
+ // Optical coordinates
+ Eigen::Vector3d graphic;
+ graphic(0) = pixel(1);
+ graphic(1) = 240 - 1 - pixel(0);
+ graphic(2) = 1.0;
- // Middle line, right to left
- mPatternPoints.push_back(Eigen::Vector4d (120.,60.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,60.,0.,1.));
- mPatternPoints.push_back(Eigen::Vector4d (0.,60.,120.,1.));
-
-
- }
+ //std::cout << "Graphic (progeo): " << graphic << std::endl;
- bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const colorspaces::Image depthData)
- {
- mPixelPoints.push_back(pixel);
-
- if (mPixelPoints.size() > mPatternPoints.size())
- return false;
+ Eigen::Vector4d p3D;
+ mProgeo->backproject(graphic, p3D);
- if (mPixelPoints.size() == mPatternPoints.size())
- {
+ //std::cout << "P3D: " << p3D << std::endl;
- std::cout << "\tPixels" << "\t\t" << "Pattern" << "\t\t" << "Camera" << std::endl;
+ Point3D *pStart = new Point3D(0.0,0.0,0.0);
+ Point3D *pEnd = new Point3D(p3D);
- for (int i=0; i<mPixelPoints.size(); i++)
- {
-
- std::cout << "P" << i << "\t("
- << mPixelPoints[i](0) << ","
- << mPixelPoints[i](1) << ") \t";
+ Segment3D *segment = new Segment3D(*pStart,*pEnd);
- std::cout << "(" << mPatternPoints[i](0) << ","
- << mPatternPoints[i](1) << ","
- << mPatternPoints[i](2) << ") \t";
+ //std::cout << "Depth: " << depth << std::endl;
- Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
+ Point3D *nP3D = segment->getPointByZ(depth);
- // We use a window of 5x5 pixels to avoid errors of the depth image
- for (int x=-2; x<=2; x++) {
+ res3D = nP3D->getPoint();
- for (int y=-2;y<=2; y++) {
- Eigen::Vector3d pixel(mPixelPoints[i](0)+x,mPixelPoints[i](1)+y,1.0);
- Eigen::Vector4d aux;
- BackProjectWithDepth (pixel, depthData, aux);
-
- if (aux(2) != 0 && aux(2) < pCamera(2)) {
- pCamera = aux;
- }
- }
- }
+ delete(segment);
+ delete(pStart);
+ delete(pEnd);
+ delete(nP3D);
- mCameraPoints.push_back(pCamera);
+}
- std::cout << "(" << pCamera[0]
- << "," << pCamera[1]
- << "," << pCamera[2] << ")"
- << std::endl;
+void Calibration::initPatternPoints()
+{
+ // Down line, right to left
+ mPatternPoints.push_back(Eigen::Vector4d (120.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (60.0,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,0.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,0.,60.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,0.,120.,1.));
+ // Up line, right to left
+ mPatternPoints.push_back(Eigen::Vector4d (120.,120.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (60.,120.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,120.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,120.,60.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,120.,120.,1.));
-
-
- }
+ // Middle line, right to left
+ mPatternPoints.push_back(Eigen::Vector4d (120.,60.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,60.,0.,1.));
+ mPatternPoints.push_back(Eigen::Vector4d (0.,60.,120.,1.));
- mPairPoints.clear();
-
- //Build pairs
- for (int i = 0; i< mPatternPoints.size(); i++)
- mPairPoints.push_back(std::make_pair(mPatternPoints[i] ,mCameraPoints[i]));
-
- for (int i = 0; i< mPatternPoints.size(); i++)
- {
- std::cout << "(" << mPairPoints[i].first(0)
- << "," << mPairPoints[i].first(1)
- << "," << mPairPoints[i].first(2) << ") -> ";
+}
- std::cout << "(" << mPairPoints[i].second(0)
- << "," << mPairPoints[i].second(1)
- << "," << mPairPoints[i].second(2) << ")"
- << std::endl;
- }
-
- LSO();
+bool Calibration::addPatternPixel (Eigen::Vector3d pixel, const colorspaces::Image depthData)
+{
+ mPixelPoints.push_back(pixel);
- mCameraPoints.clear();
- mPairPoints.clear();
+ if (mPixelPoints.size() > mPatternPoints.size())
+ return false;
- }
+ if (mPixelPoints.size() == mPatternPoints.size())
+ {
- return true;
+ std::cout << "\tPixels" << "\t\t" << "Pixel(Image)\tPixel(progeo)\tDistance" << "\t"
+ << "Backproject" << "\t\t\t" << "Fran Point" << "\t\t" << "Pattern"
+ <<std::endl;
- }
+ for (int i=0; i<mPixelPoints.size(); i++)
+ {
- void Calibration::test(Eigen::Vector4d pCamera)
- {
+ std::cout << "P" << i << "\t("
+ << mPixelPoints[i](0) << ","
+ << mPixelPoints[i](1) << ") \t";
- std::cout << "====== TEST ====== " << std::endl;
+ Eigen::Vector4d pCamera(0.,0.,1000000.,0.);
- std::cout << pCamera << std::endl;
-
- Eigen::Vector4d s = mRTsolution * pCamera;
+ Eigen::Vector3d pixelChoosen;
- std::cout << s << std::endl;
+ // We use a window of 5x5 pixels to avoid errors of the depth image
+ for (int x=-2; x<=2; x++) {
- }
+ for (int y=-2;y<=2; y++) {
+ Eigen::Vector3d pixel(mPixelPoints[i](0)+x,mPixelPoints[i](1)+y,1.0);
+ Eigen::Vector4d aux;
+ BackProjectWithDepth (pixel, depthData, aux);
- void Calibration::LSO()
- {
+ if (aux(2) != 0 && aux(2) < pCamera(2)) {
+ pCamera = aux;
+ pixelChoosen = pixel;
+ }
+ }
+ }
- gsl_matrix *ertm=gsl_matrix_alloc(4,4);
- double chisq;
- gsl_matrix *x,*cov;
- gsl_vector *y,*c;
- gsl_multifit_linear_workspace * work;
-
- x=gsl_matrix_calloc(mPairPoints.size()*3,12);
- cov=gsl_matrix_alloc(12,12);
- y=gsl_vector_alloc(mPairPoints.size()*3);
- c=gsl_vector_alloc(12);
- work = gsl_multifit_linear_alloc (mPairPoints.size()*3,12);
- double x1,y1,z1,x2,y2,z2;
- //preaparamos la matriz de ecuaciones
+ mCameraPoints.push_back(pCamera);
- for (int i=0; i<mPairPoints.size(); i++){
- x1=mPairPoints[i].first(0);
- y1=mPairPoints[i].first(1);
- z1=mPairPoints[i].first(2);
- x2=mPairPoints[i].second(0);
- y2=mPairPoints[i].second(1);
- z2=mPairPoints[i].second(2);
+ // Pixel Image
+ std::cout << "(" << pixelChoosen[0] << "," << pixelChoosen[1] << ") \t";
- gsl_matrix_set(x, i*3,0, x2);
- gsl_matrix_set(x, i*3,1, y2);
- gsl_matrix_set(x, i*3,2, z2);
- gsl_matrix_set(x, i*3,3, 1);
- gsl_vector_set(y,i*3, x1);
-
- gsl_matrix_set(x, i*3+1, 4, x2);
- gsl_matrix_set(x, i*3+1, 5, y2);
- gsl_matrix_set(x, i*3+1, 6, z2);
- gsl_matrix_set(x, i*3+1, 7, 1);
- gsl_vector_set(y,i*3+1, y1);
-
- gsl_matrix_set(x, i*3+2,8, x2);
- gsl_matrix_set(x, i*3+2,9, y2);
- gsl_matrix_set(x, i*3+2,10, z2);
- gsl_matrix_set(x, i*3+2,11, 1);
- gsl_vector_set(y,i*3+2, z1);
- }
+ mProgeo->pixel2optical(pixelChoosen);
- gsl_multifit_linear(x,y,c,cov,&chisq,work);
- gsl_multifit_linear_free(work);
-
- for (int i=0;i<3; i++) {
- for (int j=0;j<4;j++) {
- double v=c->data[i*4+j];
- if(sqrt(v*v)<0.0001)
- v=0;
- //gsl_matrix_set(ertm,i,j,v);
- mRTsolution(i,j) = v;
- }
- }
+ // Pixel Progeo
+ std::cout << "(" << pixelChoosen[0] << "," << pixelChoosen[1] << ") \t";
- /*
+ // Distance
+ float depth = (int)depthData.data[((depthData.cols*(int)pixelChoosen(1))+(int)pixelChoosen(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixelChoosen(1))+(int)pixelChoosen(0))*3+2];
+ std::cout << depth << "\t\t";
+
+ // Pixel backProject
+ Eigen::Vector4d aux3D;
+ mProgeo->backproject(pixelChoosen, aux3D);
+ std::cout << "(" << aux3D[0] << "," << aux3D[1] << "," << aux3D[2] << ")\t";
+
+
+ // fix bug about projection's progeo
+ Eigen::Vector4d p3D;
+ getRealPoint(pixelChoosen, depthData, p3D);
+ std::cout << "\t(" << p3D[0] << "," << p3D[1] << "," << p3D[2] << ")\t\t";
+
+
+ std::cout << "(" << mPatternPoints[i](0) << ","
+ << mPatternPoints[i](1) << ","
+ << mPatternPoints[i](2) << ")";
+
+ std::cout << std::endl;
+ }
+
+ mPairPoints.clear();
+
+ //Build pairs
+ for (int i = 0; i< mPatternPoints.size(); i++)
+ mPairPoints.push_back(std::make_pair(mPatternPoints[i] ,mCameraPoints[i]));
+
+
+ for (int i = 0; i< mPatternPoints.size(); i++)
+ {
+ std::cout << "(" << mPairPoints[i].first(0)
+ << "," << mPairPoints[i].first(1)
+ << "," << mPairPoints[i].first(2) << ") -> ";
+
+ std::cout << "(" << mPairPoints[i].second(0)
+ << "," << mPairPoints[i].second(1)
+ << "," << mPairPoints[i].second(2) << ")";
+
+ std::cout << std::endl;
+ }
+
+ LSO();
+
+ mCameraPoints.clear();
+ mPairPoints.clear();
+
+ }
+
+ return true;
+
+}
+
+
+void Calibration::getRealPoint(const Eigen::Vector3d pixel,
+ const colorspaces::Image depthData,
+ Eigen::Vector4d& res3D)
+{
+
+ float depth = (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+1]<<8 | (int)depthData.data[((depthData.cols*(int)pixel(1))+(int)pixel(0))*3+2];
+
+ if (depth == 0)
+ {
+ res3D(0) = 0.;
+ res3D(1) = 0.;
+ res3D(2) = 0.;
+ res3D(3) = 0.;
+ return;
+ }
+
+ Eigen::Vector4d p3D;
+ mProgeo->backproject(pixel, p3D);
+
+ //std::cout << "-> p3D: " << p3D << std::endl;
+
+ Eigen::Vector4d camPosition = mProgeo->getPosition();
+
+ //std::cout << "-> camPosition: " << camPosition << std::endl;
+
+ float module = sqrt(double(1/(((camPosition(0)-p3D(0))*(camPosition(0)-p3D(0)))+
+ ((camPosition(1)-p3D(1))*(camPosition(1)-p3D(1)))+
+ ((camPosition(2)-p3D(2))*(camPosition(2)-p3D(2))))));
+
+ //std::cout << "-> module: " << module << std::endl;
+
+ Eigen::Vector4d camFoa = mProgeo->getFoa();
+
+ //std::cout << "-> camFoa: " << camFoa << std::endl;
+
+ float fmod = sqrt(double(1/(((camPosition(0)-camFoa(0))*(camPosition(0)-camFoa(0)))+
+ ((camPosition(1)-camFoa(1))*(camPosition(1)-camFoa(1)))+
+ ((camPosition(2)-camFoa(2))*(camPosition(2)-camFoa(2))))));
+
+ //std::cout << "-> fmod: " << fmod << std::endl;
+
+ Eigen::Vector4d f;
+ f(0) = (camFoa(0)-camPosition(0)) * fmod;
+ f(1) = (camFoa(1)-camPosition(1)) * fmod;
+ f(2) = (camFoa(2)-camPosition(2)) * fmod;
+
+ //std::cout << "-> f: " << f << std::endl;
+
+ Eigen::Vector4d u;
+ u(0) = (p3D(0)-camPosition(0)) * module;
+ u(1) = (p3D(1)-camPosition(1)) * module;
+ u(2) = (p3D(2)-camPosition(2)) * module;
+
+ //std::cout << "-> u: " << u << std::endl;
+
+ Eigen::Vector4d F;
+ F(0) = depth * f(0) + camPosition(0);
+ F(1) = depth * f(1) + camPosition(1);
+ F(2) = depth * f(2) + camPosition(2);
+
+ //std::cout << "-> F: " << F << std::endl;
+
+ float t = (- (f(0)*camPosition(0)) + (f(0)*F(0)) - (f(1)*camPosition(1)) + (f(1)*F(1)) - (f(2)*camPosition(2)) + (f(2)*F(2)))/
+ ((f(0)*u(0)) + (f(1)*u(1)) + (f(2)*u(2)));
+
+ //std::cout << "t=" << t << std::endl;
+
+
+ res3D(0) = t*u(0) + camPosition(0);
+ res3D(1) = t*u(1) + camPosition(1);
+ res3D(2) = t*u(2) + camPosition(2);
+
+}
+
+
+void Calibration::test(Eigen::Vector4d pCamera)
+{
+ std::cout << "====== TEST ====== " << std::endl;
+ std::cout << pCamera << std::endl;
+
+ Eigen::Vector4d s = mRTsolution * pCamera;
+ std::cout << s << std::endl;
+}
+
+void Calibration::LSO()
+{
+
+ gsl_matrix *ertm=gsl_matrix_alloc(4,4);
+ double chisq;
+ gsl_matrix *x,*cov;
+ gsl_vector *y,*c;
+ gsl_multifit_linear_workspace * work;
+
+ x=gsl_matrix_calloc(mPairPoints.size()*3,12);
+ cov=gsl_matrix_alloc(12,12);
+ y=gsl_vector_alloc(mPairPoints.size()*3);
+ c=gsl_vector_alloc(12);
+ work = gsl_multifit_linear_alloc (mPairPoints.size()*3,12);
+ double x1,y1,z1,x2,y2,z2;
+ //preaparamos la matriz de ecuaciones
+
+ for (int i=0; i<mPairPoints.size(); i++){
+ x1=mPairPoints[i].first(0);
+ y1=mPairPoints[i].first(1);
+ z1=mPairPoints[i].first(2);
+ x2=mPairPoints[i].second(0);
+ y2=mPairPoints[i].second(1);
+ z2=mPairPoints[i].second(2);
+
+ gsl_matrix_set(x, i*3,0, x2);
+ gsl_matrix_set(x, i*3,1, y2);
+ gsl_matrix_set(x, i*3,2, z2);
+ gsl_matrix_set(x, i*3,3, 1);
+ gsl_vector_set(y,i*3, x1);
+
+ gsl_matrix_set(x, i*3+1, 4, x2);
+ gsl_matrix_set(x, i*3+1, 5, y2);
+ gsl_matrix_set(x, i*3+1, 6, z2);
+ gsl_matrix_set(x, i*3+1, 7, 1);
+ gsl_vector_set(y,i*3+1, y1);
+
+ gsl_matrix_set(x, i*3+2,8, x2);
+ gsl_matrix_set(x, i*3+2,9, y2);
+ gsl_matrix_set(x, i*3+2,10, z2);
+ gsl_matrix_set(x, i*3+2,11, 1);
+ gsl_vector_set(y,i*3+2, z1);
+ }
+
+ gsl_multifit_linear(x,y,c,cov,&chisq,work);
+ gsl_multifit_linear_free(work);
+
+ for (int i=0;i<3; i++) {
+ for (int j=0;j<4;j++) {
+ double v=c->data[i*4+j];
+ if(sqrt(v*v)<0.0001)
+ v=0;
+ //gsl_matrix_set(ertm,i,j,v);
+ mRTsolution(i,j) = v;
+ }
+ }
+
+ /*
gsl_matrix_set(ertm,3,0,0);
gsl_matrix_set(ertm,3,1,0);
gsl_matrix_set(ertm,3,2,0);
gsl_matrix_set(ertm,3,3,1);
- */
+ */
- mRTsolution(3,0) = 0.;
- mRTsolution(3,1) = 0.;
- mRTsolution(3,2) = 0.;
- mRTsolution(3,3) = 1.;
+ mRTsolution(3,0) = 0.;
+ mRTsolution(3,1) = 0.;
+ mRTsolution(3,2) = 0.;
+ mRTsolution(3,3) = 1.;
- //gsl_matrix_fprintf(stdout,ertm,"%f");
+ //gsl_matrix_fprintf(stdout,ertm,"%f");
- std::cout << mRTsolution << std::endl;
+ std::cout << mRTsolution << std::endl;
-
- }
}
+
+}
Modified: trunk/src/stable/components/rgbdCalibrator/calibration.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/calibration.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/calibration.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,11 +28,12 @@
#include <opencv2/highgui/highgui.hpp>
#include <cmath>
#include <string>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <jderobot/camera.h>
#include "../../libs/geometry/math/Point3D.h"
#include <boost/tuple/tuple.hpp>
+#include "../../libs/geometry/progeo/Progeo.h"
using namespace cv;
namespace rgbdCalibrator
@@ -73,11 +74,19 @@
Eigen::Matrix4d mRTsolution;
+ Progeo::Progeo *mProgeo;
+
std::vector<Eigen::Vector3d> mPixelPoints;
std::vector<Eigen::Vector4d> mPatternPoints;
std::vector<Eigen::Vector4d> mCameraPoints;
std::vector<std::pair<Eigen::Vector4d,Eigen::Vector4d> > mPairPoints;
+ void initProgeo();
+
+ void getRealPoint(const Eigen::Vector3d pixel,
+ const colorspaces::Image depthData,
+ Eigen::Vector4d& res3D);
+
void initPatternPoints();
void LSO();
Modified: trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/rgbdCalibrator.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -23,7 +23,7 @@
#include <Ice/Ice.h>
#include <IceUtil/IceUtil.h>
#include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include "viewer.h"
#include <cv.h>
#include <highgui.h>
Modified: trunk/src/stable/components/rgbdCalibrator/viewer.cpp
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -33,510 +33,510 @@
using namespace cvb;
namespace rgbdCalibrator{
-
-
- const std::string gladepath = std::string(GLADE_DIR) +
- std::string("/rgbdCalibrator.glade");
- const std::string pathImage = "./images/";
+const std::string gladepath = std::string(GLADE_DIR) +
+ std::string("/rgbdCalibrator.glade");
- Viewer::Viewer()
- : gtkmain(0,0),frameCount(0),
- intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL) {
+const std::string pathImage = "./images/";
- std::cout << "Loading glade\n";
- // ref widgets
- refXml = Gnome::Glade::Xml::create(gladepath);
- refXml->get_widget("color_image", gtkimage_color);
- refXml->get_widget("color_image2", gtkimage_color2);
- refXml->get_widget("depth_image", gtkimage_depth);
- refXml->get_widget("hsv_image", gtkimage_hsv);
- refXml->get_widget("blob_image", gtkimage_blob);
- refXml->get_widget("mainwindow",mainwindow);
- refXml->get_widget("fpslabel",fpsLabel);
- refXml->get_widget("bt_take_photo", btTakePhoto);
- refXml->get_widget("et_sleep_photo", etSleepPhoto);
- refXml->get_widget("et_num_photo", etNumPhoto);
- refXml->get_widget("tv_status", tvStatus);
- refXml->get_widget("bt_intrinsic_calib", btIntrinsic);
- refXml->get_widget("eventbox", ebImage);
- refXml->get_widget("eb_extrinsics", ebImageExtrinsics);
+Viewer::Viewer()
+: gtkmain(0,0),frameCount(0),
+ intrinsicsEnable(0),contPhoto(1),hsvFilter(NULL), mFrameBlob(NULL) {
- // connect signals
- btTakePhoto->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_take_photo_clicked));
+ std::cout << "Loading glade\n";
- btIntrinsic->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_intrinsic));
- ebImage->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_clicked));
+ // ref widgets
+ refXml = Gnome::Glade::Xml::create(gladepath);
+ refXml->get_widget("color_image", gtkimage_color);
+ refXml->get_widget("color_image2", gtkimage_color2);
+ refXml->get_widget("depth_image", gtkimage_depth);
+ refXml->get_widget("hsv_image", gtkimage_hsv);
+ refXml->get_widget("blob_image", gtkimage_blob);
+ refXml->get_widget("mainwindow",mainwindow);
+ refXml->get_widget("fpslabel",fpsLabel);
+ refXml->get_widget("bt_take_photo", btTakePhoto);
+ refXml->get_widget("et_sleep_photo", etSleepPhoto);
+ refXml->get_widget("et_num_photo", etNumPhoto);
+ refXml->get_widget("tv_status", tvStatus);
+ refXml->get_widget("bt_intrinsic_calib", btIntrinsic);
+ refXml->get_widget("eventbox", ebImage);
+ refXml->get_widget("eb_extrinsics", ebImageExtrinsics);
- ebImageExtrinsics->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_extrinsics_clicked));
+ // connect signals
+ btTakePhoto->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_take_photo_clicked));
- // start the timer for calculating the number of frames per second
- // the images are being displayed at
- oldFrameTime = IceUtil::Time::now();
+ btIntrinsic->signal_clicked().connect(sigc::mem_fun(this,&Viewer::on_bt_intrinsic));
+ ebImage->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_clicked));
- RGB2HSV_init();
- RGB2HSV_createTable();
+ ebImageExtrinsics->signal_button_press_event().connect(sigc::mem_fun(this, &Viewer::on_eventbox_extrinsics_clicked));
- mCalibration = new Calibration();
+ // start the timer for calculating the number of frames per second
+ // the images are being displayed at
+ oldFrameTime = IceUtil::Time::now();
- pthread_mutex_init(&mutex, NULL);
-
+ RGB2HSV_init();
+ RGB2HSV_createTable();
- }
-
+ mCalibration = new Calibration();
- Viewer::~Viewer()
- {
- if(mCalibration)
- delete(mCalibration);
+ pthread_mutex_init(&mutex, NULL);
- RGB2HSV_destroyTable();
- }
- bool Viewer::isVisible(){
- return mainwindow->is_visible();
-
- }
+}
- void Viewer::display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth )
- {
- colorspaces::ImageRGB8 img_rgb8(imageColor);//conversion will happen if needed
- Glib::RefPtr<Gdk::Pixbuf> imgBuffColor =
- Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8.data,
- Gdk::COLORSPACE_RGB,
- false,
- 8,
- img_rgb8.width,
- img_rgb8.height,
- img_rgb8.step);
-
- gtkimage_color->clear();
- gtkimage_color->set(imgBuffColor);
- gtkimage_color2->clear();
- gtkimage_color2->set(imgBuffColor);
+Viewer::~Viewer()
+{
+ if(mCalibration)
+ delete(mCalibration);
- if (intrinsicsEnable)
- saveImage(imageColor);
-
-
- pthread_mutex_lock(&mutex);
- imgOrig.create(imageColor.size(), CV_8UC3);
- imageColor.copyTo(imgOrig);
+ RGB2HSV_destroyTable();
+}
- mImageDepth = imageDepth.clone();
- pthread_mutex_unlock(&mutex);
-
- // Show depth image in color
- // 3 RGB canals
- // 0: Image in gray scale
- // [1,2]: Real data of distance
- // 1: 8bit MSB
- // 2: 8bit LSB
+bool Viewer::isVisible(){
+ return mainwindow->is_visible();
- std::vector<cv::Mat> layers;
- cv::Mat colorDepth(imageDepth.size(),imageDepth.type());
- cv::split(imageDepth, layers);
+}
- cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
+void Viewer::display( const colorspaces::Image& imageColor, const colorspaces::Image& imageDepth )
+{
+ colorspaces::ImageRGB8 img_rgb8(imageColor);//conversion will happen if needed
+ Glib::RefPtr<Gdk::Pixbuf> imgBuffColor =
+ Gdk::Pixbuf::create_from_data((const guint8*)img_rgb8.data,
+ Gdk::COLORSPACE_RGB,
+ false,
+ 8,
+ img_rgb8.width,
+ img_rgb8.height,
+ img_rgb8.step);
- Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth =
- Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,
- Gdk::COLORSPACE_RGB,
- false,
- 8,
- imageDepth.width,
- imageDepth.height,
- imageDepth.step);
+ gtkimage_color->clear();
+ gtkimage_color->set(imgBuffColor);
- gtkimage_depth->clear();
- gtkimage_depth->set(imgBuffDepth);
+ gtkimage_color2->clear();
+ gtkimage_color2->set(imgBuffColor);
- if (hsvFilter != NULL)
- {
- createImageHSV(imageDepth);
+ if (intrinsicsEnable)
+ saveImage(imageColor);
- // Show HSV image
- Glib::RefPtr<Gdk::Pixbuf> imgBuffHSV =
- Gdk::Pixbuf::create_from_data((const guint8*)imgHSV.data,
- Gdk::COLORSPACE_RGB,
- false,
- 8,
- imgHSV.size().width,
- imgHSV.size().height,
- imgHSV.step);
-
- gtkimage_hsv->clear();
- gtkimage_hsv->set(imgBuffHSV);
- // Show Blob Image
-
- Glib::RefPtr<Gdk::Pixbuf> imgBuffBLOB =
- Gdk::Pixbuf::create_from_data(
- (guint8*)mFrameBlob->imageData,
- Gdk::COLORSPACE_RGB,
- false,
- mFrameBlob->depth,
- mFrameBlob->width,
- mFrameBlob->height,
- mFrameBlob->widthStep);
+ imgOrig.create(imageColor.size(), CV_8UC3);
+ imageColor.copyTo(imgOrig);
- gtkimage_blob->clear();
- gtkimage_blob->set(imgBuffBLOB);
-
- //cvReleaseImage(&mFrameBlob);
-
- }
+ pthread_mutex_lock(&mutex);
+ mImageDepth = imageDepth.clone();
+ pthread_mutex_unlock(&mutex);
- displayFrameRate();
- mainwindow->resize(1,1);
- while (gtkmain.events_pending())
- gtkmain.iteration();
- }
-
- void Viewer::createImageHSV(const colorspaces::Image& imageDepth)
- {
- float r,g,b;
+ // Show depth image in color
+ // 3 RGB canals
+ // 0: Image in gray scale
+ // [1,2]: Real data of distance
+ // 1: 8bit MSB
+ // 2: 8bit LSB
- imgHSV.create(imgOrig.size(), CV_8UC1);
- imgOrig.copyTo(imgHSV);
+ std::vector<cv::Mat> layers;
+ cv::Mat colorDepth(imageDepth.size(),imageDepth.type());
+ cv::split(imageDepth, layers);
- IplImage *threshy=cvCreateImage(imgOrig.size(),8,1);
-
- for (int i=0;i< imgHSV.size().width*imgHSV.size().height; i++)
- {
- r = (float)(unsigned int)(unsigned char) imgOrig.data[i*3];
- g = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+1];
- b = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+2];
+ cv::cvtColor(layers[0],colorDepth,CV_GRAY2RGB);
- const HSV* hsvData = RGB2HSV_getHSV (r,g,b);
+ Glib::RefPtr<Gdk::Pixbuf> imgBuffDepth =
+ Gdk::Pixbuf::create_from_data((const guint8*) colorDepth.data,
+ Gdk::COLORSPACE_RGB,
+ false,
+ 8,
+ imageDepth.width,
+ imageDepth.height,
+ imageDepth.step);
- if( hmax >= hsvData->H*DEGTORAD && hmin <= hsvData->H*DEGTORAD
- && smax >= hsvData->S && smin <= hsvData->S
- && vmax >= hsvData->V && vmin <= hsvData->V )
- {
- threshy->imageData[i] = 1;
- }
- else
- {
- imgHSV.data[i*3] = imgHSV.data[i*3+1] = imgHSV.data[i*3+2] = 0;
- threshy->imageData[i] = 0;
- }
-
+ gtkimage_depth->clear();
+ gtkimage_depth->set(imgBuffDepth);
- }
+ if (hsvFilter != NULL)
+ {
+ createImageHSV(imageDepth);
- //Structure to hold blobs
- CvBlobs blobs;
+ // Show HSV image
+ Glib::RefPtr<Gdk::Pixbuf> imgBuffHSV =
+ Gdk::Pixbuf::create_from_data((const guint8*)imgHSV.data,
+ Gdk::COLORSPACE_RGB,
+ false,
+ 8,
+ imgHSV.size().width,
+ imgHSV.size().height,
+ imgHSV.step);
- IplImage *iplOrig = new IplImage(imgOrig);
+ gtkimage_hsv->clear();
+ gtkimage_hsv->set(imgBuffHSV);
- if (mFrameBlob)
- cvReleaseImage(&mFrameBlob);
- mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
+ // Show Blob Image
- IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
+ Glib::RefPtr<Gdk::Pixbuf> imgBuffBLOB =
+ Gdk::Pixbuf::create_from_data(
+ (guint8*)mFrameBlob->imageData,
+ Gdk::COLORSPACE_RGB,
+ false,
+ mFrameBlob->depth,
+ mFrameBlob->width,
+ mFrameBlob->height,
+ mFrameBlob->widthStep);
- cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
+ gtkimage_blob->clear();
+ gtkimage_blob->set(imgBuffBLOB);
- //Threshy is a binary image
- cvSmooth(threshy,threshy,CV_MEDIAN,7,7);
+ //cvReleaseImage(&mFrameBlob);
- //Finding the blobs
- unsigned int result=cvLabel(threshy,labelImg,blobs);
+ }
- //Rendering the blobs
- cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
+ displayFrameRate();
+ mainwindow->resize(1,1);
+ while (gtkmain.events_pending())
+ gtkmain.iteration();
+}
- //Filter Blobs
- cvFilterByArea(blobs,500,5000);
+void Viewer::createImageHSV(const colorspaces::Image& imageDepth)
+{
+ float r,g,b;
- double area = 0.0;
- int x=0;
- int y=0;
-
- for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
- {
- //std::cout << "BLOB found: " << it->second->area <<std::endl;
+ imgHSV.create(imgOrig.size(), CV_8UC1);
+ imgOrig.copyTo(imgHSV);
- double moment10 = it->second->m10;
- double moment01 = it->second->m01;
+ IplImage *threshy=cvCreateImage(imgOrig.size(),8,1);
- if (it->second->area >= area)
- {
- area = it->second->area;
- x = moment10/area;
- y = moment01/area;
- }
-
- }
+ for (int i=0;i< imgHSV.size().width*imgHSV.size().height; i++)
+ {
+ r = (float)(unsigned int)(unsigned char) imgOrig.data[i*3];
+ g = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+1];
+ b = (float)(unsigned int)(unsigned char) imgOrig.data[i*3+2];
- std::cout << "Max BLOB: " << area << ": " << x << " , " << y <<std::endl;
+ const HSV* hsvData = RGB2HSV_getHSV (r,g,b);
- //cvShowImage("Live",mFrameBlob);
+ if( hmax >= hsvData->H*DEGTORAD && hmin <= hsvData->H*DEGTORAD
+ && smax >= hsvData->S && smin <= hsvData->S
+ && vmax >= hsvData->V && vmin <= hsvData->V )
+ {
+ threshy->imageData[i] = 1;
+ }
+ else
+ {
+ imgHSV.data[i*3] = imgHSV.data[i*3+1] = imgHSV.data[i*3+2] = 0;
+ threshy->imageData[i] = 0;
+ }
- if (area != 0)
- {
- Eigen::Vector3d pixel;
- pixel(0) = x;
- pixel(1) = y;
- pixel(2) = 1.0;
-
- Eigen::Vector4d target;
-
- mCalibration->BackProjectWithDepth(pixel, imageDepth, target);
- }
+ }
- // Release and free memory
- delete(iplOrig);
- cvReleaseImage(&threshy);
- cvReleaseImage(&labelImg);
+ //Structure to hold blobs
+ CvBlobs blobs;
- }
+ IplImage *iplOrig = new IplImage(imgOrig);
- void
- Viewer::displayFrameRate()
- {
- double diff;
- IceUtil::Time diffT;
+ if (mFrameBlob)
+ cvReleaseImage(&mFrameBlob);
+ mFrameBlob=cvCreateImage(imgOrig.size(),8,3);
- currentFrameTime = IceUtil::Time::now();
- diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
- if (diff < 1000.0)
- frameCount++;
- else{
- oldFrameTime = currentFrameTime;
- fps = frameCount*1000.0/diff;
- frameCount=0;
- // Display the frame rate
- std::stringstream fpsString;
- fpsString << "fps = " << int(fps);
- fpsLabel->set_label(fpsString.str());
- }
- }
+ IplImage *labelImg=cvCreateImage(imgOrig.size(),IPL_DEPTH_LABEL,1);
- bool Viewer::on_eventbox_extrinsics_clicked(GdkEventButton * event)
- {
+ cvResize(iplOrig,mFrameBlob,CV_INTER_LINEAR );
- if (mCalibration){
-
- pthread_mutex_lock(&mutex);
- bool res = mCalibration->addPatternPixel (Eigen::Vector3d ((int) event->x,
- (int) event->y,
- 1.0),
- mImageDepth);
+ //Threshy is a binary image
+ cvSmooth(threshy,threshy,CV_MEDIAN,7,7);
- if (res == false)
- {
- Eigen::Vector3d p2D ((int) event->x, (int) event->y, 1.0);
- Eigen::Vector4d p3D;
- mCalibration->BackProjectWithDepth (p2D, mImageDepth, p3D);
- mCalibration->test(p3D);
- }
+ //Finding the blobs
+ unsigned int result=cvLabel(threshy,labelImg,blobs);
- pthread_mutex_unlock(&mutex);
- }
- }
+ //Rendering the blobs
+ cvRenderBlobs(labelImg,blobs,mFrameBlob,mFrameBlob);
- bool Viewer::on_eventbox_clicked(GdkEventButton * event)
- {
- int posX, posY;
- float r,g,b;
- posX = (int) event->x;
- posY = (int) event->y;
-
- pthread_mutex_lock(&mutex);
-
- int index = posY*imgOrig.step+posX*imgOrig.channels();
- r = (float)(unsigned int) (unsigned char)imgOrig.data[index];
- g = (float)(unsigned int) (unsigned char)imgOrig.data[index+1];
- b = (float)(unsigned int) (unsigned char)imgOrig.data[index+2];
-
- pthread_mutex_unlock(&mutex);
-
-
- if (DEBUG) std::cout << "[RGB] -> " << r << " " << g << " " << b << std::endl;
- hsvFilter = RGB2HSV_getHSV (r,g,b);
- if (DEBUG) std::cout << "[HSV] -> " << hsvFilter->H << " " << hsvFilter->S << " " << hsvFilter->V << std::endl;
-
- // Calculate HSV Min y Max
- hmax = hsvFilter->H*DEGTORAD + 0.2;
- hmin = hsvFilter->H*DEGTORAD - 0.2;
- if(hmax>6.28) hmax = 6.28;
- if(hmin<0.0) hmin = 0.0;
-
- smax = hsvFilter->S + 0.1;
- smin = hsvFilter->S - 0.1;
- if(smax > 1.0)
- smax = 1.0;
- if(smin < 0.0)
- smin = 0.0;
-
- vmax = hsvFilter->V + 50.0;
- vmin = hsvFilter->V - 50.0;
- if(vmax > 255.0)
- vmax = 255.0;
- if(vmin < 0.0)
- vmin = 0.0;
-
- if (DEBUG)
- std::cout << "H[min,max] - S[min,max] - V[min,max]: " <<
- "[" << hmin << " " << hmax << "] " <<
- "[" << smin << " " << smax << "] " <<
- "[" << vmin << " " << vmax << "] " << std::endl;
-
- return true;
- }
-
- void Viewer::on_bt_take_photo_clicked()
- {
- intrinsicsEnable = 1;
+ //Filter Blobs
+ cvFilterByArea(blobs,500,5000);
- // Get num of photos
- std::string num (etNumPhoto->get_buffer()->get_text());
- numPhoto = atoi(num.c_str());
- std::cout << numPhoto << std::endl;
+ double area = 0.0;
+ int x=0;
+ int y=0;
- // Get delay photo and Init time last photo
- std::string sleep (etSleepPhoto->get_buffer()->get_text());
- delayPhoto = atoi(sleep.c_str());
- lastTimePhoto = IceUtil::Time::now();
+ for (CvBlobs::const_iterator it=blobs.begin(); it!=blobs.end(); ++it)
+ {
+ //std::cout << "BLOB found: " << it->second->area <<std::endl;
- tvStatus->get_buffer()->set_text("Init process, show the pattern!");
- }
+ double moment10 = it->second->m10;
+ double moment01 = it->second->m01;
- void Viewer::saveImage(const colorspaces::Image& imageColor)
- {
-
+ if (it->second->area >= area)
+ {
+ area = it->second->area;
+ x = moment10/area;
+ y = moment01/area;
+ }
- IceUtil::Time currentTime = IceUtil::Time::now();
- if ((currentTime - lastTimePhoto).toSeconds() == delayPhoto)
- {
+ }
- // Check the directory
- if ( !boost::filesystem::exists(pathImage))
- {
- boost::filesystem::path dir(pathImage);
- if (!boost::filesystem::create_directory(dir))
- std::cout << "Error to create directory" << std::endl;
- }
+ std::cout << "Max BLOB: " << area << ": " << x << " , " << y <<std::endl;
+ //cvShowImage("Live",mFrameBlob);
- lastTimePhoto = IceUtil::Time::now();
+ if (area != 0)
+ {
+ Eigen::Vector3d pixel;
+ pixel(0) = x;
+ pixel(1) = y;
+ pixel(2) = 1.0;
- // Convert to gray
- Mat gray(imageColor.size(), CV_8UC1);
- cvtColor(imageColor, gray, CV_RGB2GRAY);
+ Eigen::Vector4d target;
- // Save Image
- std::stringstream filename;
- filename << pathImage << "img" << contPhoto << ".jpg";
- imwrite( filename.str().c_str(), gray );
+ mCalibration->BackProjectWithDepth(pixel, imageDepth, target);
- std::string msg = "Image saved: " + filename.str();
- tvStatus->get_buffer()->set_text(msg.c_str());
-
- beep();
+ }
- if (contPhoto < numPhoto)
- contPhoto ++;
- else
+ // Release and free memory
+ delete(iplOrig);
+ cvReleaseImage(&threshy);
+ cvReleaseImage(&labelImg);
+
+}
+
+void
+Viewer::displayFrameRate()
+{
+ double diff;
+ IceUtil::Time diffT;
+
+ currentFrameTime = IceUtil::Time::now();
+ diff = (currentFrameTime - oldFrameTime).toMilliSecondsDouble();
+ if (diff < 1000.0)
+ frameCount++;
+ else{
+ oldFrameTime = currentFrameTime;
+ fps = frameCount*1000.0/diff;
+ frameCount=0;
+ // Display the frame rate
+ std::stringstream fpsString;
+ fpsString << "fps = " << int(fps);
+ fpsLabel->set_label(fpsString.str());
+ }
+}
+
+bool Viewer::on_eventbox_extrinsics_clicked(GdkEventButton * event)
+{
+
+ if (mCalibration){
+
+ pthread_mutex_lock(&mutex);
+ bool res = mCalibration->addPatternPixel (Eigen::Vector3d ((int) event->x,
+ (int) event->y,
+ 1.0),
+ mImageDepth);
+
+ if (res == false)
+ {
+ Eigen::Vector3d p2D ((int) event->x, (int) event->y, 1.0);
+ Eigen::Vector4d p3D;
+ mCalibration->BackProjectWithDepth (p2D, mImageDepth, p3D);
+ mCalibration->test(p3D);
+ }
+
+ pthread_mutex_unlock(&mutex);
+ }
+}
+
+bool Viewer::on_eventbox_clicked(GdkEventButton * event)
+{
+ int posX, posY;
+ float r,g,b;
+ posX = (int) event->x;
+ posY = (int) event->y;
+
+ pthread_mutex_lock(&mutex);
+
+ int index = posY*imgOrig.step+posX*imgOrig.channels();
+ r = (float)(unsigned int) (unsigned char)imgOrig.data[index];
+ g = (float)(unsigned int) (unsigned char)imgOrig.data[index+1];
+ b = (float)(unsigned int) (unsigned char)imgOrig.data[index+2];
+
+ pthread_mutex_unlock(&mutex);
+
+
+ if (DEBUG) std::cout << "[RGB] -> " << r << " " << g << " " << b << std::endl;
+ hsvFilter = RGB2HSV_getHSV (r,g,b);
+ if (DEBUG) std::cout << "[HSV] -> " << hsvFilter->H << " " << hsvFilter->S << " " << hsvFilter->V << std::endl;
+
+ // Calculate HSV Min y Max
+ hmax = hsvFilter->H*DEGTORAD + 0.2;
+ hmin = hsvFilter->H*DEGTORAD - 0.2;
+ if(hmax>6.28) hmax = 6.28;
+ if(hmin<0.0) hmin = 0.0;
+
+ smax = hsvFilter->S + 0.1;
+ smin = hsvFilter->S - 0.1;
+ if(smax > 1.0)
+ smax = 1.0;
+ if(smin < 0.0)
+ smin = 0.0;
+
+ vmax = hsvFilter->V + 50.0;
+ vmin = hsvFilter->V - 50.0;
+ if(vmax > 255.0)
+ vmax = 255.0;
+ if(vmin < 0.0)
+ vmin = 0.0;
+
+ if (DEBUG)
+ std::cout << "H[min,max] - S[min,max] - V[min,max]: " <<
+ "[" << hmin << " " << hmax << "] " <<
+ "[" << smin << " " << smax << "] " <<
+ "[" << vmin << " " << vmax << "] " << std::endl;
+
+ return true;
+}
+
+void Viewer::on_bt_take_photo_clicked()
+{
+ intrinsicsEnable = 1;
+
+ // Get num of photos
+ std::string num (etNumPhoto->get_buffer()->get_text());
+ numPhoto = atoi(num.c_str());
+ std::cout << numPhoto << std::endl;
+
+ // Get delay photo and Init time last photo
+ std::string sleep (etSleepPhoto->get_buffer()->get_text());
+ delayPhoto = atoi(sleep.c_str());
+ lastTimePhoto = IceUtil::Time::now();
+
+ tvStatus->get_buffer()->set_text("Init process, show the pattern!");
+}
+
+void Viewer::saveImage(const colorspaces::Image& imageColor)
+{
+
+
+ IceUtil::Time currentTime = IceUtil::Time::now();
+ if ((currentTime - lastTimePhoto).toSeconds() == delayPhoto)
{
- intrinsicsEnable = 0;
- contPhoto = 1;
- }
- }
- }
+ // Check the directory
+ if ( !boost::filesystem::exists(pathImage))
+ {
+ boost::filesystem::path dir(pathImage);
+ if (!boost::filesystem::create_directory(dir))
+ std::cout << "Error to create directory" << std::endl;
+ }
- void Viewer::beep()
- {
- system("mplayer beep.wav &> /dev/null");
- }
-
- void Viewer::on_bt_intrinsic()
- {
+ lastTimePhoto = IceUtil::Time::now();
- tvStatus->get_buffer()->set_text("Getting intrinsics ...");
+ // Convert to gray
+ Mat gray(imageColor.size(), CV_8UC1);
+ cvtColor(imageColor, gray, CV_RGB2GRAY);
- Size boardSize, imageSize;
- Mat cameraMatrix, distCoeffs;
- vector<vector<Point2f> > imagePoints;
+ // Save Image
+ std::stringstream filename;
+ filename << pathImage << "img" << contPhoto << ".jpg";
+ imwrite( filename.str().c_str(), gray );
+ std::string msg = "Image saved: " + filename.str();
+ tvStatus->get_buffer()->set_text(msg.c_str());
- // TODO: get this data from user by GUI
- boardSize.width = 9;
- boardSize.height= 6;
+ beep();
- //flag
- int flag = 0;
- flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
- flag |= CV_CALIB_ZERO_TANGENT_DIST;
- flag |= CV_CALIB_FIX_ASPECT_RATIO;
+ if (contPhoto < numPhoto)
+ contPhoto ++;
+ else
+ {
+ intrinsicsEnable = 0;
+ contPhoto = 1;
+ }
+ }
- // List of images
- boost::filesystem::directory_iterator end_itr;
- for ( boost::filesystem::directory_iterator itr( pathImage ); itr != end_itr; ++itr )
- {
+}
- Mat view;
- view = imread(itr->path().c_str(), CV_LOAD_IMAGE_COLOR);
+void Viewer::beep()
+{
+ system("mplayer beep.wav &> /dev/null");
+}
- imageSize = view.size();
+void Viewer::on_bt_intrinsic()
+{
- vector<Point2f> pointBuf;
- bool found = findChessboardCorners( view, boardSize, pointBuf,
- CV_CALIB_CB_ADAPTIVE_THRESH |
- CV_CALIB_CB_FAST_CHECK |
- CV_CALIB_CB_NORMALIZE_IMAGE);
- if (found)
- {
-
- Mat viewGray;
- cvtColor(view, viewGray, CV_BGR2GRAY);
- cornerSubPix( viewGray, pointBuf, Size(11,11),
- Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
+ tvStatus->get_buffer()->set_text("Getting intrinsics ...");
- imagePoints.push_back(pointBuf);
+ Size boardSize, imageSize;
+ Mat cameraMatrix, distCoeffs;
+ vector<vector<Point2f> > imagePoints;
- // Draw the corners.
- drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
- //imshow("Image View", view);
-
- }
- }
+ // TODO: get this data from user by GUI
+ boardSize.width = 9;
+ boardSize.height= 6;
- mCalibration->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,
- cameraMatrix, distCoeffs, imagePoints);
+ //flag
+ int flag = 0;
+ flag |= CV_CALIB_FIX_PRINCIPAL_POINT;
+ flag |= CV_CALIB_ZERO_TANGENT_DIST;
+ flag |= CV_CALIB_FIX_ASPECT_RATIO;
- std::cout << std::endl << cameraMatrix << std::endl;
+ // List of images
+ boost::filesystem::directory_iterator end_itr;
+ for ( boost::filesystem::directory_iterator itr( pathImage ); itr != end_itr; ++itr )
+ {
- std::stringstream matrixStr;
+ Mat view;
+ view = imread(itr->path().c_str(), CV_LOAD_IMAGE_COLOR);
- matrixStr << "Intrinsic Matrix" << std::endl;
- matrixStr << "================" << std::endl << std::endl;
+ imageSize = view.size();
- for (int i=0; i<cameraMatrix.cols ; i++)
- {
- for (int j=0; j<cameraMatrix.rows; j++)
- {
- matrixStr << cameraMatrix.at<double>(i,j) << "\t\t";
- if (i==2 && j==0)
- matrixStr << "\t";
- }
- matrixStr<<std::endl;
+ vector<Point2f> pointBuf;
+ bool found = findChessboardCorners( view, boardSize, pointBuf,
+ CV_CALIB_CB_ADAPTIVE_THRESH |
+ CV_CALIB_CB_FAST_CHECK |
+ CV_CALIB_CB_NORMALIZE_IMAGE);
- tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
- }
+ if (found)
+ {
-
- }
+ Mat viewGray;
+ cvtColor(view, viewGray, CV_BGR2GRAY);
+ cornerSubPix( viewGray, pointBuf, Size(11,11),
+ Size(-1,-1), TermCriteria( CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1 ));
+ imagePoints.push_back(pointBuf);
+ // Draw the corners.
+ drawChessboardCorners( view, boardSize, Mat(pointBuf), found );
+ //imshow("Image View", view);
+
+ }
+ }
+
+
+ mCalibration->runCalibrationAndSave(boardSize, 20.0, flag, imageSize,
+ cameraMatrix, distCoeffs, imagePoints);
+
+ std::cout << std::endl << cameraMatrix << std::endl;
+
+ std::stringstream matrixStr;
+
+ matrixStr << "Intrinsic Matrix" << std::endl;
+ matrixStr << "================" << std::endl << std::endl;
+
+ for (int i=0; i<cameraMatrix.cols ; i++)
+ {
+ for (int j=0; j<cameraMatrix.rows; j++)
+ {
+ matrixStr << cameraMatrix.at<double>(i,j) << "\t\t";
+ if (i==2 && j==0)
+ matrixStr << "\t";
+ }
+ matrixStr<<std::endl;
+
+ tvStatus->get_buffer()->set_text(matrixStr.str().c_str());
+ }
+
+
+}
+
+
}//namespace
Modified: trunk/src/stable/components/rgbdCalibrator/viewer.h
===================================================================
--- trunk/src/stable/components/rgbdCalibrator/viewer.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdCalibrator/viewer.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -28,10 +28,10 @@
#include <IceUtil/Thread.h>
#include <IceUtil/Time.h>
#include <string>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <cmath>
#include <jderobot/camera.h>
-#include <colorspaces/colorspaces.h>
+#include <visionlib/colorspaces/colorspaces.h>
#include "calibration.h"
using namespace cv;
Modified: trunk/src/stable/components/rgbdManualCalibrator/controller.h
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/controller.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdManualCalibrator/controller.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -29,7 +29,7 @@
#include <iostream>
#include <opencv/cv.h>
#include <progeo/progeo.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <Ice/Ice.h>
#include <IceUtil/IceUtil.h>
#include <opencv/cv.h>
Modified: trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdManualCalibrator/rgbdManualCalibrator.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -25,7 +25,7 @@
#include <Ice/Ice.h>
#include <IceUtil/IceUtil.h>
#include <jderobot/camera.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include "view.h"
#include "controller.h"
#include "common.h"
Modified: trunk/src/stable/components/rgbdManualCalibrator/view.h
===================================================================
--- trunk/src/stable/components/rgbdManualCalibrator/view.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/components/rgbdManualCalibrator/view.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -30,7 +30,7 @@
#include <IceUtil/Thread.h>
#include <IceUtil/Time.h>
#include "controller.h"
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include "drawarea.h"
#include "common.h"
//eclipse
Modified: trunk/src/stable/libs/geometry/math/Segment3D.cpp
===================================================================
--- trunk/src/stable/libs/geometry/math/Segment3D.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/geometry/math/Segment3D.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -84,7 +84,7 @@
delete(vector);
- Point3D* res = new Point3D(x,y,z);
+ Point3D* res = new Point3D(x,y,z,1.0);
return res;
}
Modified: trunk/src/stable/libs/geometry/progeo/Progeo.cpp
===================================================================
--- trunk/src/stable/libs/geometry/progeo/Progeo.cpp 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/geometry/progeo/Progeo.cpp 2013-10-07 13:37:13 UTC (rev 1012)
@@ -24,6 +24,7 @@
#include "Progeo.h"
#include <math.h>
+
namespace Progeo {
Progeo::Progeo()
@@ -46,6 +47,7 @@
foa(3) = 1.;
roll = 0.;
+ skew = 0.;
}
@@ -357,7 +359,7 @@
void Progeo::displayCameraInfo() {
- printf("------------------------------------------------------\n");
+ printf("\n----------------------- PROGEO C++ ---------------------\n");
printf("Camera %s\n\n", this->name.c_str());
printf(" Position: (X,Y,Z,H)=(%.1f,%.1f,%.1f,%.1f)\n", position(0),
position(1),
@@ -408,14 +410,14 @@
void Progeo::backproject(Eigen::Vector3d point, Eigen::Vector4d& pro)
{
//GRAPHIC_TO_OPTICAL
- int opX = this->rows -1 -point(1);
- int opY = point(0);
+ //int opX = this->rows -1 -point(1);
+ //int opY = point(0);
Eigen::Matrix3d ik;
ik = K;
ik = ik.inverse().eval();
- Eigen::Vector3d Pi(opX*K(0,0)/point(2), opY*K(0,0)/point(2),K(0,0));
+ Eigen::Vector3d Pi(pro(0)*K(0,0)/point(2), pro(1)*K(0,0)/point(2),K(0,0));
Eigen::Vector3d a;
a = ik*Pi;
@@ -634,9 +636,6 @@
xmlDocSetRootElement(doc, root_node);
- //updateKMatrix();
- //updateRTMatrix();
-
// Camera position
xmlNewChild(node_position, NULL, BAD_CAST "x", BAD_CAST double2char(position(0)));
xmlNewChild(node_position, NULL, BAD_CAST "y", BAD_CAST double2char(position(1)));
Modified: trunk/src/stable/libs/parallelIce/cameraClient.h
===================================================================
--- trunk/src/stable/libs/parallelIce/cameraClient.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/parallelIce/cameraClient.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -25,7 +25,7 @@
#include <IceUtil/IceUtil.h>
#include <iostream>
#include <Ice/Ice.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include <jderobot/camera.h>
#include <cv.h>
#include <sstream>
Modified: trunk/src/stable/libs/visionlib/CMakeLists.txt
===================================================================
--- trunk/src/stable/libs/visionlib/CMakeLists.txt 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/visionlib/CMakeLists.txt 2013-10-07 13:37:13 UTC (rev 1012)
@@ -1,3 +1,6 @@
+
+add_subdirectory (colorspaces)
+
SET( SOURCE_FILES cvfast.cpp geometry.cpp image.cpp linesDetection.cpp)
include_directories( ${LIBS_DIR}/)
add_library (visionlib SHARED ${SOURCE_FILES})
Modified: trunk/src/stable/libs/visionlib/colorspaces/CMakeLists.txt
===================================================================
--- trunk/src/stable/libs/colorspaces/CMakeLists.txt 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/visionlib/colorspaces/CMakeLists.txt 2013-10-07 13:37:13 UTC (rev 1012)
@@ -1,6 +1,4 @@
-
-
add_library (colorspacesmm STATIC imagecv.cpp imagecv.h uncopyable.h)
TARGET_LINK_LIBRARIES(colorspacesmm ${OpenCV_LIBRARIES})
Modified: trunk/src/stable/libs/visionlib/linesDetection.h
===================================================================
--- trunk/src/stable/libs/visionlib/linesDetection.h 2013-10-07 12:49:18 UTC (rev 1011)
+++ trunk/src/stable/libs/visionlib/linesDetection.h 2013-10-07 13:37:13 UTC (rev 1012)
@@ -27,7 +27,7 @@
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <stdio.h>
-#include <colorspaces/colorspacesmm.h>
+#include <visionlib/colorspaces/colorspacesmm.h>
#include "geometry.h"
#include "cvfast.h"
#include "structs.h"
More information about the Jderobot-admin
mailing list