void GrahamScanConvexHull::graham_scan(std::vector< point2d >& final_hull)
{
const std::size_t HEAD = 0;
const std::size_t PRE_HEAD = 1;
std::deque<gs_point2d> pnt_queue;
pnt_queue.push_front(point[0]);
pnt_queue.push_front(point[1]);
unsigned int i = 2;
while(i < point.size())
{
if (pnt_queue.size() > 1)
{
if (orientation(pnt_queue[PRE_HEAD],pnt_queue[HEAD],point[i]) == counter_clock_wise)
pnt_queue.push_front(point[i++]);
else
pnt_queue.pop_front();
}
else
pnt_queue.push_front(point[i++]);
}
for(std::deque<gs_point2d>::iterator it = pnt_queue.begin(); it != pnt_queue.end(); it++)
{
final_hull.push_back(point2d((*it).x, (*it).y));
}
}
shape makecircle(double radius, point2d centre)
{
const point2d xaxis(radius, 0);
const point2d xaxis_plus (radius, 1);
const double angleForOnePixel = direction (point2d(0,0), xaxis_plus);
const int numVertexes = (int)(360.0 / angleForOnePixel);
shape retVal = make_regular_polygon(radius, numVertexes);
moveshape(retVal, centre);
return retVal;
}
Ball::Ball(int px, int py, int sx, int sy, int r) {
/*! \brief Initialize vectors via initialization list
*/
x = 0;
radius = r;
offset = vector2d(px, py);
slope = vector2d(sx, sy);
center = point2d(px, py);
}
// .Explode(ent) - Will create an explosion entity at ent's position
// This will not remove ent from the map!
int entity_Explode(lua_State* ls)
{
Image* img;
rect r;
Entity* e = _getReferencedEntity(ls);
if (e)
{
img = e->GetImage();
if (img)
{
r = e->GetBoundingRect();
new ExplodingEntity(e->mMap, img, point2d(r.x, r.y));
}
}
return 0;
}
shape make_regular_polygon(double radius, int n)
{
shape retVal;
double delta = radians(360.0) / n;
double currentAngle = 0;
while (n--)
{
retVal.push_back(point2d(radius * cos(currentAngle), radius * sin(currentAngle)));
currentAngle += delta;
}
//now close it.
if (retVal.size())
{
retVal.push_back (retVal.at(0));
}
return retVal;
}
void ActorNameTag::Update()
{
rect r;
rect rr;
// reposition self relative to owner
if (mOwner)
{
if (mText != mOwner->mName)
{
mText = mOwner->mName;
}
r = mOwner->GetBoundingRect();
rr = GetBoundingRect();
r.y -= rr.h + 2;
r.x += r.w / 2 - (rr.w / 2);
SetPosition(point2d(r.x, r.y));
}
}
// キャリブレーション
__declspec(dllexport) void calcCalibration( double srcPoint2dx[], double srcPoint2dy[], double srcPoint3dx[], double srcPoint3dy[], double srcPoint3dz[], int srcCorrespond, int proWidth, int proHeight, double projectionMatrix[], double externalMatrix[], double& reprojectionResult)
{
cv::Mat point2dx(1, srcCorrespond, CV_64F, srcPoint2dx);
cv::Mat point2dy(1, srcCorrespond, CV_64F, srcPoint2dy);
cv::Mat point3dx(1, srcCorrespond, CV_64F, srcPoint3dx);
cv::Mat point3dy(1, srcCorrespond, CV_64F, srcPoint3dy);
cv::Mat point3dz(1, srcCorrespond, CV_64F, srcPoint3dz);
// 対応点
cv::vector<cv::Point2d> imagePoints;
cv::vector<cv::Point3d> worldPoints;
for(int i = 0; i < srcCorrespond; ++i)
{
cv::Point2d point2d(point2dx.at<double>(i), point2dy.at<double>(i));
cv::Point3d point3d(point3dx.at<double>(i), point3dy.at<double>(i), point3dz.at<double>(i));
imagePoints.push_back(point2d);
worldPoints.push_back(point3d);
}
/////////////////// 透視投影変換行列の推定 /////////////////////////////////
cv::Mat perspectiveMat; // 透視投影変換行列
// 透視投影変換行列の推定
calcProjectionMatrix(worldPoints, imagePoints, perspectiveMat);
// 再投影誤差
reprojectionResult = inspection_error_value(perspectiveMat, worldPoints, imagePoints);
perspectiveMat = perspectiveMat/perspectiveMat.at<double>(11);
cv::Mat cameraMat;
cv::Mat rotation;
cv::Mat translate;
cv::Mat translate2 = cv::Mat::eye(3,1,CV_64F);
cv::Mat worldTranslate = cv::Mat::eye(3,1,CV_64F);
cv::decomposeProjectionMatrix(perspectiveMat, cameraMat, rotation, translate); // 透視投影変換行列の分解→あやしい
// OpenGL用内部パラメータ
cameraMat = -cameraMat / cameraMat.at<double>(8);
double far = 1000.0;
double near = 0.05;
cv::Mat cameraMatrix0 = cv::Mat::zeros(4, 4, CV_64F);
cv::Mat cameraMatrix = cv::Mat::zeros(4, 4, CV_64F);
cameraMatrix0.at<double>(0) = cameraMat.at<double>(0);
cameraMatrix0.at<double>(1) = cameraMat.at<double>(1);
cameraMatrix0.at<double>(2) = cameraMat.at<double>(2);
cameraMatrix0.at<double>(5) = cameraMat.at<double>(4);
cameraMatrix0.at<double>(6) = cameraMat.at<double>(5);
cameraMatrix0.at<double>(10) = -(far+near) / (far-near);
cameraMatrix0.at<double>(11) = -2*far*near / (far-near);
cameraMatrix0.at<double>(14) = cameraMat.at<double>(8);
cv::Mat M = cv::Mat::eye(4, 4, CV_64F);
M.at<double>(0) = 2.0 / (double)proWidth;
M.at<double>(3) = -1;
M.at<double>(5) = -2.0 / (double)proHeight;
M.at<double>(7) = 1;
cameraMatrix = M * cameraMatrix0;
cameraMatrix.at<double>(5) = -cameraMatrix.at<double>(5);
// 外部パラメータ
translate2.at<double>(0) = translate.at<double>(0)/translate.at<double>(3);
translate2.at<double>(1) = translate.at<double>(1)/translate.at<double>(3);
translate2.at<double>(2) = translate.at<double>(2)/translate.at<double>(3);
wornldTranslate = rotation * translate2; //decomposeProjectionMatrix()のせい？
//decomposeProjectionMatrix()のせいで-色々つけてる？
cv::Mat externalMat = cv::Mat::eye(4,4,CV_64F);
externalMat.at<double>(0) = rotation.at<double>(0);
externalMat.at<double>(1) = rotation.at<double>(1);
externalMat.at<double>(2) = rotation.at<double>(2);
externalMat.at<double>(3) = -worldTranslate.at<double>(0);
externalMat.at<double>(4) = -rotation.at<double>(3);
externalMat.at<double>(5) = -rotation.at<double>(4);
externalMat.at<double>(6) = -rotation.at<double>(5);
externalMat.at<double>(7) = worldTranslate.at<double>(1);
externalMat.at<double>(8) = rotation.at<double>(6);
externalMat.at<double>(9) = rotation.at<double>(7);
externalMat.at<double>(10) = rotation.at<double>(8);
externalMat.at<double>(11) = -worldTranslate.at<double>(2);
// 結果の保存
cv::FileStorage fs2("Calibration/calibration.xml", cv::FileStorage::WRITE);
cv::write(fs2,"reprojectionError", reprojectionResult);
cv::write(fs2,"projectorCalibration", perspectiveMat);
cv::write(fs2,"internalMatrix", cameraMat);
cv::write(fs2,"cameraMatrix", cameraMatrix);
cv::write(fs2,"externalMatrix", externalMat);
for(int i = 0; i < 16; ++i)
{
projectionMatrix[i] = cameraMatrix.at<double>(i);
externalMatrix[i] = externalMat.at<double>(i);
}
}
void Reconstruction::update (vector<Matcher::p_match> p_matched,Matrix Tr,int32_t point_type,int32_t min_track_length,double max_dist,double min_angle) {
// update transformation vector
Matrix Tr_total_curr;
if (Tr_total.size()==0) Tr_total_curr = Matrix::inv(Tr);
else Tr_total_curr = Tr_total.back()*Matrix::inv(Tr);
Tr_total.push_back(Tr_total_curr);
Tr_inv_total.push_back(Matrix::inv(Tr_total_curr));
// update projection vector
Matrix P_total_curr = K*Matrix::inv(Tr_total_curr).getMat(0,0,2,3);
P_total.push_back(P_total_curr);
// current frame
int32_t current_frame = Tr_total.size() - 1; // 0-based frame number
// create index vector
int32_t track_idx_max = 0;
for (vector<Matcher::p_match>::iterator m=p_matched.begin(); m!=p_matched.end(); m++)
if (m->i1p > track_idx_max)
track_idx_max = m->i1p;
for (vector<track>::iterator t=tracks.begin(); t!=tracks.end(); t++)
if (t->last_idx > track_idx_max)
track_idx_max = t->last_idx;
int32_t *track_idx = new int32_t[track_idx_max+1];
for (int32_t i=0; i<=track_idx_max; i++)
track_idx[i] = -1;
for (int32_t i=0; i<tracks.size(); i++)
track_idx[tracks[i].last_idx] = i;
// associate matches to tracks
for (vector<Matcher::p_match>::iterator m=p_matched.begin(); m!=p_matched.end(); m++) {
// track index (-1 = no existing track)
int32_t idx = track_idx[m->i1p];
// add to existing track
if (idx>=0 && tracks[idx].last_frame==current_frame-1) {
tracks[idx].pixels.push_back(point2d(m->u1c,m->v1c));
tracks[idx].last_frame = current_frame;
tracks[idx].last_idx = m->i1c;
// create new track
} else {
track t;
t.pixels.push_back(point2d(m->u1p,m->v1p));
t.pixels.push_back(point2d(m->u1c,m->v1c));
t.first_frame = current_frame-1;
t.last_frame = current_frame;
t.last_idx = m->i1c;
tracks.push_back(t);
}
}
// copy tracks
vector<track> tracks_copy = tracks;
tracks.clear();
// devise tracks into active or reconstruct 3d points
for (vector<track>::iterator t=tracks_copy.begin(); t!=tracks_copy.end(); t++) {
// track has been extended
if (t->last_frame==current_frame) {
// push back to active tracks
tracks.push_back(*t);
// track lost
} else {
// add to 3d reconstruction
if (t->pixels.size()>=min_track_length) {
// 3d point
point3d p;
// try to init point from first and last track frame
if (initPoint(*t,p)) {
if (pointType(*t,p)>=point_type)
if (refinePoint(*t,p))
if(pointDistance(*t,p)<max_dist && rayAngle(*t,p)>min_angle)
points.push_back(p);
}
}
}
}
//cout << "P: " << points.size() << endl;
//testJacobian();
delete track_idx;
}
point2d shift_linear(float _x,float _y) {return point2d(x+_x,y+_y);};
point2d shift_radial(float radius,float angle) {return point2d(x+cos(angle)*radius,y+sin(angle)*radius);};
