double LightplaneCalibrator::Calibrate() {
GeneratePointCloud();
cv::Mat pcloud(light_point_cloud_.size(), 3, CV_64F);
cv::Mat light_plane_wcs(1,4,CV_64F);
double x_ave, y_ave, z_ave;
for (int i = 0; i < light_point_cloud_.size(); i++) {
pcloud.row(i) = light_point_cloud_[i].t();
}
x_ave = mean(pcloud.col(0))(0);
y_ave = mean(pcloud.col(1))(0);
z_ave = mean(pcloud.col(2))(0);
pcloud.col(0) = pcloud.col(0) - x_ave;
pcloud.col(1) = pcloud.col(1) - y_ave;
pcloud.col(2) = pcloud.col(2) - z_ave;
cv::Mat w, u, vt;
cv::SVD::compute(pcloud, w, u, vt);
vt.row(2).copyTo(light_plane_wcs.colRange(0, 3));
light_plane_wcs.at<double>(0, 3) = -
light_plane_wcs.at<double>(0, 0) * x_ave -
light_plane_wcs.at<double>(0, 1) * y_ave -
light_plane_wcs.at<double>(0, 2) * z_ave;
cam_->light_plane_ = light_plane_wcs * ref_pose_[0].inv();
return 0.0;
}
void KINECT_BASE::UpdateData(VISION_DATA &data)
{
mKinectStruct->mStatus = mKinectStruct->mContext.WaitAndUpdateAll();
//mKinectStruct->CheckOpenNIError(mKinectStruct->mStatus, "View Cloud");
memset(&data, 0, sizeof(data));
const XnDepthPixel* pDepthMap = mKinectStruct->mDepthGenerator.GetDepthMap();
memcpy(data.depthMap, pDepthMap, 480*640*sizeof(unsigned short));
GeneratePointCloud(mKinectStruct->mDepthGenerator, pDepthMap, data);
/*
// Experiment Robot
Matrix4f kinectAdjust;
kinectAdjust<< -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
Matrix4f kinectToRobot;
kinectToRobot<<0.9995, 0.0134, -0.0273, 0.0224, -0.0304, 0.5120, -0.8584, 0.2026 + 0.038,
0.0025, 0.8589, 0.5122, 0.5733, 0, 0, 0, 1;
*/
// Experiment Desk
Matrix4f kinectAdjust;
kinectAdjust<< -1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
Matrix4f kinectToRobot;
kinectToRobot<< 0.9989836, -0.001837780, -0.0450375377, 0.017682,
-0.038951129, 0.4676363607, -0.88306231020, 0.197015+0.038,
0.02268406498, 0.88391903289, 0.46708947376, 0.561919,
0, 0, 0, 1;
Matrix4f robotToWorld;
robotToWorld << 1, 0, 0, 0,
0, 1, 0, 0.85,
0, 0, 1, 0,
0, 0, 0, 1;
Matrix4f kinectToWorld = robotToWorld*kinectToRobot*kinectAdjust;
ofstream ofs1;
stringstream out1;
out1<<frameNum;
string dataname1 ="../PointCloud/originalcloud" + out1.str() + ".txt";
ofs1.open(dataname1,ios::trunc);
ofstream ofs2;
stringstream out2;
out2<<frameNum;
string dataname2 ="../PointCloud/transformedcloud_" + out2.str() + ".txt";
ofs2.open(dataname2,ios::trunc);
ofstream ofs3;
stringstream out3;
out3<<frameNum;
string dataname3 ="../PointCloud/grid_" + out3.str() + ".txt";
ofs3.open(dataname3,ios::trunc);
for (int i = 0; i < 480; i++)
{
for(int j = 0; j < 640; j++)
{
ofs1<<data.pointCloud[i][j][0]/1000<<" "<<data.pointCloud[i][j][1]/1000<<" "<<data.pointCloud[i][j][2]/1000<<" "<<endl;
}
}
for (int i = 0; i < 480; i++)
{
for(int j = 0; j < 640; j++)
{
// if(data.pointCloud[i][j][0] != 0&&data.pointCloud[i][j][1] != 0&&data.pointCloud[i][j][2] != 0)
if(data.pointCloud[i][j][2] != 0)
{
Point3D tempPoint = {0, 0, 0};
tempPoint.X = kinectToWorld(0, 0)*data.pointCloud[i][j][0] + kinectToWorld(0, 1)*data.pointCloud[i][j][1]
+ kinectToWorld(0, 2)*data.pointCloud[i][j][2] + kinectToWorld(0, 3)*1000;
tempPoint.Y = kinectToWorld(1, 0)*data.pointCloud[i][j][0] + kinectToWorld(1, 1)*data.pointCloud[i][j][1]
+ kinectToWorld(1, 2)*data.pointCloud[i][j][2] + kinectToWorld(1, 3)*1000;
tempPoint.Z = kinectToWorld(2, 0)*data.pointCloud[i][j][0] + kinectToWorld(2, 1)*data.pointCloud[i][j][1]
+ kinectToWorld(2, 2)*data.pointCloud[i][j][2] + kinectToWorld(2, 3)*1000;
data.pointCloud[i][j][0] = tempPoint.X/1000;
data.pointCloud[i][j][1] = tempPoint.Y/1000;
data.pointCloud[i][j][2] = tempPoint.Z/1000;
}
ofs2<<data.pointCloud[i][j][0]<<" "<<data.pointCloud[i][j][1]<<" "<<data.pointCloud[i][j][2]<<" "<<endl;
}
}
GenerateGridMap(data);
for(int i = 0; i < 60; i++)
{
for(int j = 0; j < 60; j++)
{
ofs3<<data.pGridMap[i][j].X<<" "<<data.pGridMap[i][j].Y<<" "<<data.pGridMap[i][j].Z<<endl;
}
}
cout<<"FrameNum: "<<frameNum<<endl;
ofs1.close();
ofs2.close();
ofs3.close();
}
void KINECT_BASE::updateData(VISION_DATA &data)
{
mKinectStruct->mStatus = mKinectStruct->mContext.WaitAndUpdateAll();
//mKinectStruct->CheckOpenNIError(mKinectStruct->mStatus, "View Cloud");
memset(&data, 0, sizeof(data));
const XnDepthPixel* pDepthMap = mKinectStruct->mDepthGenerator.GetDepthMap();
memcpy(data.depthMap, pDepthMap, 480*640*sizeof(unsigned short));
GeneratePointCloud(mKinectStruct->mDepthGenerator, pDepthMap, data);
Matrix4f kinectAdjust;
kinectAdjust<< 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
// Matrix4f kinectToRobot;
// kinectToRobot<< 0.9989836, -0.001837780, -0.0450375377, 0.017682,
// -0.038951129, 0.4676363607, -0.88306231020, 0.197015+0.038,
// 0.02268406498, 0.88391903289, 0.46708947376, 0.561919,
// 0, 0, 0, 1;
// Matrix4f kinectToRobot;
// kinectToRobot<< 1, 0, 0, 0.017682,
// 0, 0.866, -0.5, 0.197015+0.038+0.85,
// 0, 0.5, 0.866, 0,
// 0, 0, 0, 1;
//Kinect On Desk
//Matrix4f kinectToRobot;
//kinectToRobot<< 1, 0, 0, 0,
//0, 0.891, -0.454, 0.797,
//0, 0.454, 0.891, 0,
//0, 0, 0, 1;
//Kinect On Robot
Matrix4f kinectToRobot;
kinectToRobot<<
1, 0, 0, 0,
0, 0.8746, -0.4848, 0.9610,
0, 0.4848, 0.8746, 0,
0, 0, 0, 1;
Matrix4f robotToWorld;
robotToWorld << 1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1;
// robotToWorld << 1, 0, 0, 0,
// 0, 1, 0, 0,
// 0, 0, 1, 0,
// 0, 0, 0, 1;
Matrix4f kinectToWorld = robotToWorld*kinectToRobot*kinectAdjust;
// write point cloud data
// ofstream ofs1;
// stringstream out1;
// out1<<frameNum;
// string dataname1 ="../PointCloud/originalcloud" + out1.str() + ".txt";
// ofs1.open(dataname1,ios::trunc);
// ofstream ofs2;
// stringstream out2;
// out2<<frameNum;
// string dataname2 ="../PointCloud/transformedcloud_" + out2.str() + ".txt";
// ofs2.open(dataname2,ios::trunc);
// ofstream ofs3;
// stringstream out3;
// out3<<frameNum;
// string dataname3 ="../PointCloud/grid_" + out3.str() + ".txt";
// ofs3.open(dataname3,ios::trunc);
// for (int i = 0; i < 480; i++)
// {
// for(int j = 0; j < 640; j++)
// {
// ofs1<<data.pointCloud[i][j][0]/1000<<" "<<data.pointCloud[i][j][1]/1000<<" "<<data.pointCloud[i][j][2]/1000<<" "<<endl;
// }
// }
for (int i = 0; i < 480; i++)
{
for(int j = 0; j < 640; j++)
{
if(data.pointCloud[i][j][2] != 0)
{
Point3D tempPoint = {0, 0, 0};
tempPoint.X = kinectToWorld(0, 0)*data.pointCloud[i][j][0] + kinectToWorld(0, 1)*data.pointCloud[i][j][1]
+ kinectToWorld(0, 2)*data.pointCloud[i][j][2] + kinectToWorld(0, 3)*1000;
tempPoint.Y = kinectToWorld(1, 0)*data.pointCloud[i][j][0] + kinectToWorld(1, 1)*data.pointCloud[i][j][1]
+ kinectToWorld(1, 2)*data.pointCloud[i][j][2] + kinectToWorld(1, 3)*1000;
tempPoint.Z = kinectToWorld(2, 0)*data.pointCloud[i][j][0] + kinectToWorld(2, 1)*data.pointCloud[i][j][1]
+ kinectToWorld(2, 2)*data.pointCloud[i][j][2] + kinectToWorld(2, 3)*1000;
data.pointCloud[i][j][0] = tempPoint.X/1000;
data.pointCloud[i][j][1] = tempPoint.Y/1000;
data.pointCloud[i][j][2] = tempPoint.Z/1000;
}
// ofs2<<data.pointCloud[i][j][0]<<" "<<data.pointCloud[i][j][1]<<" "<<data.pointCloud[i][j][2]<<" "<<endl;
}
}
GenerateGridMap(data);
GenerateObstacleMap(data);
// for(int i = 0; i < 60; i++)
// {
// for(int j = 0; j < 60; j++)
// {
// ofs3<<data.pGridMap[i][j].X<<" "<<data.pGridMap[i][j].Y<<" "<<data.pGridMap[i][j].Z<<endl;
// }
// }
// cout<<"FrameNum: "<<frameNum<<endl;
// ofs1.close();
// ofs2.close();
// ofs3.close();
}
