Skip to content

Commit

Permalink
major update; fix bugs on stereoMatch and UI operation
Browse files Browse the repository at this point in the history
  • Loading branch information
@yuhuazou
yuhuazou committed Apr 15, 2013
1 parent e05a704 commit c77cc11
Show file tree
Hide file tree
Showing 11 changed files with 745 additions and 338 deletions.
14 changes: 8 additions & 6 deletions StereoVision/PointCloudAnalyzer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -121,25 +121,27 @@ void PointCloudAnalyzer::parseCandidates(cv::Mat& objects, cv::Mat& depthMap, ve
mask = cv::Scalar(0);
drawContours(mask, contours, objID, cv::Scalar(255), -1);

// 计算轮廓矩形
object.boundRect = boundingRect( contour );
object.minRect = minAreaRect( contour );
object.center = object.minRect.center;

// 计算物体深度
if (useMeanDepth) //取平均深度
{
cv::Scalar meanVal = cv::mean(depthMap, mask);
object.distance = meanVal[0];
object.nearest = object.center;
}
else //取最近深度
{
double minVal = 0, maxVal = 0;
cv::Point minPos;
cv::minMaxLoc(depthMap, &minVal, &maxVal, &minPos, NULL, mask);
cv::minMaxLoc(depthMap, &minVal, &maxVal, &minPos, NULL, mask);
object.nearest = minPos;
object.distance = depthMap.at<float>(minPos.y, minPos.x);
}

// 计算轮廓矩形
object.boundRect = boundingRect( contour );
object.minRect = minAreaRect( contour );
object.center = object.minRect.center;

// 保存物体轮廓信息
objectInfos.push_back( object );
}
Expand Down
4 changes: 3 additions & 1 deletion StereoVision/PointCloudAnalyzer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ class PointCloudAnalyzer
struct ObjectInfo
{
cv::Point center; //中心
cv::Point nearest; //最近点
double distance; //距离
double area; //面积
cv::Rect boundRect; //外接矩形
Expand All @@ -39,7 +40,8 @@ class PointCloudAnalyzer
// 定义赋值操作
void operator = (const ObjectInfo& rhs)
{
center = rhs.center;
center = rhs.center;
nearest = rhs.nearest;
distance = rhs.distance;
area = rhs.area;
boundRect = rhs.boundRect;
Expand Down
19 changes: 10 additions & 9 deletions StereoVision/StereoCalib.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -298,7 +298,7 @@ int StereoCalib::detectCorners(cv::Mat& img1, cv::Mat& img2, CornerDatas& corner
*/
void StereoCalib::showText(cv::Mat& img, string text)
{
int fontFace = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
int fontFace = cv::FONT_HERSHEY_PLAIN;
double fontScale = 1;
int fontThickness = 2;

Expand All @@ -315,16 +315,16 @@ void StereoCalib::showText(cv::Mat& img, string text)
// draw the box
rectangle(img, textOrg + cv::Point(0, textBaseline),
textOrg + cv::Point(textSize.width, -textSize.height),
cv::Scalar(0,0,255));
cv::Scalar(0,255,0));

// ... and the textBaseline first
line(img, textOrg + cv::Point(0, fontThickness),
textOrg + cv::Point(textSize.width, fontThickness),
cv::Scalar(0, 0, 255));
cv::Scalar(0,255,0));

// then put the text itself
putText(img, text, textOrg, fontFace, fontScale,
cv::Scalar::all(255), fontThickness, 8);
cv::Scalar(0,0,255), fontThickness, 8);
}


Expand Down Expand Up @@ -398,7 +398,7 @@ int StereoCalib::saveCameraParams(const CameraParams& cameraParams, const char*
std::string filename_ = filename;

//按当前时间生成文件名
if (filename_ == "" || filename_ == "cameraParams.yml")
if (filename_ == "")
{
int strLen = 20;
char *pCurrTime = (char*)malloc(sizeof(char)*strLen);
Expand Down Expand Up @@ -540,8 +540,8 @@ int StereoCalib::calibrateStereoCamera(CornerDatas& cornerDatas, StereoParams& s
/***
* 保存单目定标结果至本地
*/
saveCameraParams(stereoParams.cameraParams1, "cameraParams_left.yml"/*待改为由本地设置文件确定*/);
saveCameraParams(stereoParams.cameraParams2, "cameraParams_right.yml"/*待改为由本地设置文件确定*/);
saveCameraParams(stereoParams.cameraParams1, (m_workDir + "cameraParams_left.yml").c_str());
saveCameraParams(stereoParams.cameraParams2, (m_workDir + "cameraParams_right.yml").c_str());
}

stereoParams.imageSize = cornerDatas.imageSize;
Expand Down Expand Up @@ -731,7 +731,8 @@ int StereoCalib::rectifyStereoCamera(CornerDatas& cornerDatas, StereoParams& ste

cv::Mat R1, R2, P1, P2, Q;
cv::Rect roi1, roi2;
double alpha = -1;
if (stereoParams.alpha < 0 || stereoParams.alpha > 1)
stereoParams.alpha = -1;

//执行双目校正
stereoRectify(
Expand All @@ -744,7 +745,7 @@ int StereoCalib::rectifyStereoCamera(CornerDatas& cornerDatas, StereoParams& ste
stereoParams.translation,
R1,R2, P1, P2, Q,
CV_CALIB_ZERO_DISPARITY,
alpha,
stereoParams.alpha,
stereoParams.imageSize,
&roi1, &roi2);

Expand Down
9 changes: 7 additions & 2 deletions StereoVision/StereoCalib.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,7 @@ class StereoCalib
cv::Mat essential; // 本质矩阵
cv::Mat foundational; // 基础矩阵
int flags; // 双目标定所用的标志位
double alpha; // 双目校正效果的缩放系数，取值 0~1 或 -1
};

/***
Expand All @@ -88,6 +89,8 @@ class StereoCalib
*/
enum RECTIFYMETHOD { RECTIFY_BOUGUET, RECTIFY_HARTLEY };

void setWorkDir(const char* workDir) { m_workDir = workDir; }

/*----------------------------
* 功能 : 初始化棋盘角点数据信息
*----------------------------
Expand Down Expand Up @@ -286,8 +289,6 @@ class StereoCalib
*/
int remapImage(cv::Mat& img1, cv::Mat& img2, cv::Mat& img1r, cv::Mat& img2r, RemapMatrixs& remapMatrixs);

private:

/*----------------------------
* 功能 : 在图像右下角显示指定文字信息
*----------------------------
Expand All @@ -300,6 +301,10 @@ class StereoCalib
*/
void showText(cv::Mat& img, string text);

private:

string m_workDir; // 工作目录

};

#endif
126 changes: 106 additions & 20 deletions StereoVision/StereoMatch.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,9 +89,7 @@ int StereoMatch::loadCalibData(const char* xmlFilePath)
cv::Mat lfCamMat;
fs["leftCameraMatrix"] >> lfCamMat;
m_FL = lfCamMat.at<double>(0,0);

m_Calib_Mat_Q.at<double>(3, 2) = -m_Calib_Mat_Q.at<double>(3, 2);


m_Calib_Mat_Mask_Roi = cv::Mat::zeros(m_frameHeight, m_frameWidth, CV_8UC1);
cv::rectangle(m_Calib_Mat_Mask_Roi, m_Calib_Roi_L, cv::Scalar(255), -1);

Expand Down Expand Up @@ -291,22 +289,17 @@ int StereoMatch::getPointClouds(cv::Mat& disparity, cv::Mat& pointClouds)
//计算生成三维点云
cv::reprojectImageTo3D(disparity, pointClouds, m_Calib_Mat_Q, true);

// 坐标数据校正
// 乘以 16 得到 毫米 单位坐标，乘以 1.6 得到 厘米 单位坐标
// 原理参见：http://blog.csdn.net/chenyusiyuan/article/details/5967291
pointClouds *= 1.6;

// 校正 Y 方向数据，正负反转
// 原理参见：http://blog.csdn.net/chenyusiyuan/article/details/5970799
for (int y = 0; y < pointClouds.rows; ++y)
{
for (int x = 0; x < pointClouds.cols; ++x)
{
cv::Point3f point = pointClouds.at<cv::Point3f>(y,x);
point.y = -point.y;
pointClouds.at<cv::Point3f>(y,x) = point;
}
}
//// 校正 Y 方向数据，正负反转
//// 原理参见：http://blog.csdn.net/chenyusiyuan/article/details/5970799
//for (int y = 0; y < pointClouds.rows; ++y)
//{
// for (int x = 0; x < pointClouds.cols; ++x)
// {
// cv::Point3f point = pointClouds.at<cv::Point3f>(y,x);
// point.y = -point.y;
// pointClouds.at<cv::Point3f>(y,x) = point;
// }
//}

return 1;
}
Expand Down Expand Up @@ -339,7 +332,7 @@ int StereoMatch::getDisparityImage(cv::Mat& disparity, cv::Mat& disparityImage,
// 转换为伪彩色图像 或 灰度图像
if (isColor)
{
if (disparityImage.empty() || disparityImage.type() != CV_8UC3 )
if (disparityImage.empty() || disparityImage.type() != CV_8UC3 || disparityImage.size() != disparity.size())
{
disparityImage = cv::Mat::zeros(disparity.rows, disparity.cols, CV_8UC3);
}
Expand Down Expand Up @@ -373,6 +366,99 @@ int StereoMatch::getDisparityImage(cv::Mat& disparity, cv::Mat& disparityImage,
}


/*----------------------------
* 功能 : 获取环境俯视图
*----------------------------
* 函数 : StereoMatch::savePointClouds
* 访问 : public
* 返回 : void
*
* 参数 : pointClouds [in] 三维点云数据
* 参数 : topDownView [out] 环境俯视图
* 参数 : image [in] 环境图像
*/
void StereoMatch::getTopDownView(cv::Mat& pointClouds, cv::Mat& topDownView, cv::Mat& image /*= cv::Mat()*/)
{
int VIEW_WIDTH = m_nViewWidth, VIEW_DEPTH = m_nViewDepth;
cv::Size mapSize = cv::Size(VIEW_DEPTH, VIEW_WIDTH);

if (topDownView.empty() || topDownView.size() != mapSize || topDownView.type() != CV_8UC3)
topDownView = cv::Mat(mapSize, CV_8UC3);

topDownView = cv::Scalar::all(50);

if (pointClouds.empty())
return;

if (image.empty() || image.size() != pointClouds.size())
image = 255 * cv::Mat::ones(pointClouds.size(), CV_8UC3);

for(int y = 0; y < pointClouds.rows; y++)
{
for(int x = 0; x < pointClouds.cols; x++)
{
cv::Point3f point = pointClouds.at<cv::Point3f>(y, x);
int pos_Z = point.z;

if ((0 <= pos_Z) && (pos_Z < VIEW_DEPTH))
{
int pos_X = point.x + VIEW_WIDTH/2;
if ((0 <= pos_X) && (pos_X < VIEW_WIDTH))
{
topDownView.at<cv::Vec3b>(pos_X,pos_Z) = image.at<cv::Vec3b>(y,x);
}
}
}
}
}

/*----------------------------
* 功能 : 获取环境俯视图
*----------------------------
* 函数 : StereoMatch::savePointClouds
* 访问 : public
* 返回 : void
*
* 参数 : pointClouds [in] 三维点云数据
* 参数 : sideView [out] 环境侧视图
* 参数 : image [in] 环境图像
*/
void StereoMatch::getSideView(cv::Mat& pointClouds, cv::Mat& sideView, cv::Mat& image /*= cv::Mat()*/)
{
int VIEW_HEIGTH = m_nViewHeight, VIEW_DEPTH = m_nViewDepth;
cv::Size mapSize = cv::Size(VIEW_DEPTH, VIEW_HEIGTH);

if (sideView.empty() || sideView.size() != mapSize || sideView.type() != CV_8UC3)
sideView = cv::Mat(mapSize, CV_8UC3);

sideView = cv::Scalar::all(50);

if (pointClouds.empty())
return;

if (image.empty() || image.size() != pointClouds.size())
image = 255 * cv::Mat::ones(pointClouds.size(), CV_8UC3);

for(int y = 0; y < pointClouds.rows; y++)
{
for(int x = 0; x < pointClouds.cols; x++)
{
cv::Point3f point = pointClouds.at<cv::Point3f>(y, x);
int pos_Y = point.y + VIEW_HEIGTH/2;
int pos_Z = point.z;

if ((0 <= pos_Z) && (pos_Z < VIEW_DEPTH))
{
if ((0 <= pos_Y) && (pos_Y < VIEW_HEIGTH))
{
sideView.at<cv::Vec3b>(pos_Y,pos_Z) = image.at<cv::Vec3b>(y,x);
}
}
}
}
}


/*----------------------------
* 功能 : 保存三维点云到本地 txt 文件
*----------------------------
Expand Down
48 changes: 48 additions & 0 deletions StereoVision/StereoMatch.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,50 @@ class StereoMatch
* 参数 : filename [in] 文件路径
*/
void savePointClouds(cv::Mat& pointClouds, const char* filename);

/*----------------------------
* 功能 : 设置视场范围
*----------------------------
* 函数 : StereoMatch::setViewField
* 访问 : public
* 返回 : void
*
* 参数 : viewWidth [in] 视场宽度
* 参数 : viewHeight [in] 视场高度
* 参数 : viewDepth [in] 视场深度
*/
void setViewField(int viewWidth, int viewHeight, int viewDepth)
{
m_nViewWidth = viewWidth;
m_nViewHeight = viewHeight;
m_nViewDepth = viewDepth;
}

/*----------------------------
* 功能 : 获取环境俯视图
*----------------------------
* 函数 : StereoMatch::savePointClouds
* 访问 : public
* 返回 : void
*
* 参数 : pointClouds [in] 三维点云数据
* 参数 : topDownView [out] 环境俯视图
* 参数 : image [in] 环境图像
*/
void getTopDownView(cv::Mat& pointClouds, cv::Mat& topDownView, cv::Mat& image = cv::Mat());

/*----------------------------
* 功能 : 获取环境俯视图
*----------------------------
* 函数 : StereoMatch::savePointClouds
* 访问 : public
* 返回 : void
*
* 参数 : pointClouds [in] 三维点云数据
* 参数 : sideView [out] 环境侧视图
* 参数 : image [in] 环境图像
*/
void getSideView(cv::Mat& pointClouds, cv::Mat& sideView, cv::Mat& image = cv::Mat());

/***
* 公开变量
Expand Down Expand Up @@ -149,6 +193,10 @@ class StereoMatch
int m_frameHeight; // 帧高
int m_numberOfDisparies; // 视差变化范围

int m_nViewWidth; // 视场宽度
int m_nViewHeight; // 视场高度
int m_nViewDepth; // 视场深度

};

#endif
Binary file modified StereoVision/StereoVision.aps
View file
Binary file not shown.
Loading

0 comments on commit c77cc11

Please sign in to comment.