void PolygonSliceGenerator::compute(BaseGeometry* geo, unsigned int i, const Point& p)
{
const unsigned int pps = getNumVerticesPerSlice();
const unsigned int np(_polygon.size());
osg::Quat q;
q.makeRotate(osg::Y_AXIS, p.up);
for(unsigned int j = 0; j < np; ++j)
{
osg::Vec3 d(
_polygon[j][0] * p.width * 0.5,
_polygon[j][1] * p.height * 0.5,
0);
d = q * d;
osg::Vec3 v_x(p.hN * d.x());
osg::Vec3 v_y(p.vN * d.y());
(*_vertices)[i * pps + j] = p.point + v_x + v_y;
osg::Vec3 n(p.vN);
if (pps > 2)
n = (*_vertices)[i * pps + j] - p.point;
n.normalize();
(*_normals)[i * pps + j] = n;
}
if (i > 0)
{
unsigned int i1,i2,i3,i4;
unsigned int start_ndx = isClosed() ? 0 : 1;
unsigned int num_indices_per_slice = getNumIndicesPerSlice();
for(unsigned int j=start_ndx; j < _polygon.size(); ++j)
{
unsigned int base_ndx = (i-1) * num_indices_per_slice + (j-start_ndx)*6;
if (isClosed()) {
i1 = (i-1) * np + j;
i2 = (i-1) * np + (j+1) % np;
i3 = (i-0) * np + j;
i4 = (i-0) * np + (j+1) % np;
} else {
i1 = (i-1) * np + j;
i2 = (i-1) * np + (j-1);
i3 = (i-0) * np + j;
i4 = (i-0) * np + (j-1);
}
(*_indices)[base_ndx + 0] = i1;
(*_indices)[base_ndx + 1] = i2;
(*_indices)[base_ndx + 2] = i3;
(*_indices)[base_ndx + 3] = i3;
(*_indices)[base_ndx + 4] = i2;
(*_indices)[base_ndx + 5] = i4;
}
}
}
bool CShape::GetExtents(double* extents, const double* orig, const double* xdir, const double* ydir, const double* zdir)
{
gp_Pnt p_orig(0, 0, 0);
if(orig)p_orig = gp_Pnt(orig[0], orig[1], orig[2]);
gp_Vec v_x(1, 0, 0);
if(xdir)v_x = gp_Vec(xdir[0], xdir[1], xdir[2]);
gp_Vec v_y(0, 1, 0);
if(ydir)v_y = gp_Vec(ydir[0], ydir[1], ydir[2]);
gp_Vec v_z(0, 0, 1);
if(zdir)v_z = gp_Vec(zdir[0], zdir[1], zdir[2]);
BRepPrimAPI_MakeBox cuboid_plus_x(gp_Ax2(gp_Pnt(p_orig.XYZ() + 2000000 * v_x.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_y.XYZ()), v_x, v_y), 1000000, 1000000, 1000000);
BRepPrimAPI_MakeBox cuboid_minus_x(gp_Ax2(gp_Pnt(p_orig.XYZ() + (-2000000) * v_x.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_y.XYZ()), -v_x, v_z), 1000000, 1000000, 1000000);
BRepPrimAPI_MakeBox cuboid_plus_y(gp_Ax2(gp_Pnt(p_orig.XYZ() + 2000000 * v_y.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_x.XYZ()), v_y, v_z), 1000000, 1000000, 1000000);
BRepPrimAPI_MakeBox cuboid_minus_y(gp_Ax2(gp_Pnt(p_orig.XYZ() + (-2000000) * v_y.XYZ() + (-1000000) * v_z.XYZ() + (-1000000) * v_x.XYZ()), -v_y, v_x), 1000000, 1000000, 1000000);
BRepPrimAPI_MakeBox cuboid_plus_z(gp_Ax2(gp_Pnt(p_orig.XYZ() + 2000000 * v_z.XYZ() + (-1000000) * v_x.XYZ() + (-1000000) * v_y.XYZ()), v_z, v_x), 1000000, 1000000, 1000000);
BRepPrimAPI_MakeBox cuboid_minus_z(gp_Ax2(gp_Pnt(p_orig.XYZ() + (-2000000) * v_z.XYZ() + (-1000000) * v_x.XYZ() + (-1000000) * v_y.XYZ()), -v_z, v_y), 1000000, 1000000, 1000000);
gp_Vec v_orig(p_orig.XYZ());
TopoDS_Solid shape[6] =
{
cuboid_minus_x,
cuboid_minus_y,
cuboid_minus_z,
cuboid_plus_x,
cuboid_plus_y,
cuboid_plus_z
};
gp_Vec vector[6] =
{
v_x,
v_y,
v_z,
v_x,
v_y,
v_z
};
for(int i = 0; i<6; i++){
BRepExtrema_DistShapeShape extrema(m_shape, shape[i]);
extrema.Perform();
gp_Pnt p = extrema.PointOnShape1(1);
gp_Vec v(p.XYZ());
double dp = v * vector[i];
double dp_o = v_orig * vector[i];
extents[i] = dp - dp_o;
}
return true;
}
int main(int argc,char** argv)
{
cv::Mat src = cv::imread("input.jpg");
IplImage* img=cvLoadImage("input.jpg");
IplImage* image = cvCloneImage(img);
IplImage* temp=cvCloneImage(image);
cvNamedWindow("Draw Line Example", CV_WINDOW_NORMAL);
cvSetMouseCallback(
"Draw Line Example",
my_callback,
(void*)image);
while(1)
{
cvCopyImage(image,temp);
if(drawing_line) draw_line(temp,pt1,pt2);
cvShowImage("Draw Line Example",temp);
int c = cv::waitKey(20);
if ( c == 'q' )
{
break;
}
switch(c){
case 'x':
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 'y':
x_point_vector = point_vector;
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 'z':
y_point_vector = point_vector;
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 's':
z_point_vector = point_vector;
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 'h':
point_vector.clear();
drawing_point = true;
cvCopyImage(img,image);
break;
default:
break;
}
}
float w = (img->width + img->height)/4.0;
// calculate vanishing points
for (std::vector<int>::size_type i=0;i!= x_point_vector.size();++i){
x_point_vector[i].x -= img->width/2;
x_point_vector[i].y -= img->height/2;
}
for (std::vector<int>::size_type i=0;i!= y_point_vector.size();++i){
y_point_vector[i].x -= img->width/2;
y_point_vector[i].y -= img->height/2;
}
for (std::vector<int>::size_type i=0;i!= z_point_vector.size();++i){
z_point_vector[i].x -= img->width/2;
z_point_vector[i].y -= img->height/2;
}
CvPoint3D32f x_vanish = calc_vanishing_point(x_point_vector,w);
CvPoint3D32f y_vanish = calc_vanishing_point(y_point_vector,w);
CvPoint3D32f z_vanish = calc_vanishing_point(z_point_vector,w);
point_vector.clear();
point_vector.push_back(cvPoint(2316, 3681));
point_vector.push_back(cvPoint(1711, 3372));
point_vector.push_back(cvPoint(2226, 3300));
point_vector.push_back(cvPoint(2942, 3570));
point_vector.push_back(cvPoint(2326, 3092));
// calculate homography of x-y plane and scale in xyz axis
if (point_vector.size() < 5)
{
std::cout << "Less than 5 points are clicked!\n";
return -1;
}
double scale_x = 300, scale_y = 600, scale_z = 300;
std::vector <cv::Point2d> target_point_vector;
target_point_vector.push_back(cv::Point2d(0,0));
target_point_vector.push_back(cv::Point2d(0, scale_y));
target_point_vector.push_back(cv::Point2d(scale_x, scale_y));
target_point_vector.push_back(cv::Point2d(scale_x, 0));
std::vector <cv::Point2d> src_point_vector;
src_point_vector.push_back(cv::Point2d(point_vector[0].x, point_vector[0].y));
src_point_vector.push_back(cv::Point2d(point_vector[1].x, point_vector[1].y));
src_point_vector.push_back(cv::Point2d(point_vector[2].x, point_vector[2].y));
src_point_vector.push_back(cv::Point2d(point_vector[3].x, point_vector[3].y));
cv::Vec3d origin(point_vector[0].x, point_vector[0].y, 1);
cv::Mat Hxy = cv::findHomography(src_point_vector, target_point_vector);
cv::Vec3d z_scale(point_vector[4].x, point_vector[4].y, 1);
point_vector.clear();
//cv::Vec3d v_x(x_vanish.x + image->width / 2, x_vanish.y + image->height / 2, 1);
//cv::Vec3d v_y(y_vanish.x + image->width / 2, y_vanish.y + image->height / 2, 1);
//cv::Vec3d v_z(z_vanish.x + image->width / 2, z_vanish.y + image->height / 2, 1);
cv::Vec3d v_x(9563.7, 2403.48, 1);
cv::Vec3d v_y(-86.75, 2409.9, 1);
cv::Vec3d v_z(5534.12, 530677, 1);
std::cout << "origin: " << origin << "\n";
std::cout << "vanishing x: " << v_x << "\n";
std::cout << "vanishing y: " << v_y << "\n";
std::cout << "vanishing z: " << v_z << "\n";
std::cout << "homography: " << Hxy << "\n";
// calculate scale of z axis
double z_temp = cal3dZ(v_x, v_y, v_z, z_scale, origin, origin, 1);
scale_z = scale_z / z_temp;
double x_coord, y_coord, z_coord;
cv::Vec3d pt0, pt1, pt2, pt3;
cv::Vec3d top0, bottom0, top1, bottom1, top2, bottom2, top3, bottom3;
std::vector<cv::Vec3d> src_text_vector;
std::vector<cv::Vec3d> target_text_vector;
int cnt = 111;
while (1)
{
cvCopyImage(image, temp);
int c = cv::waitKey(20);
if (c == 27)
{
break;
}
if (c == 'p')
{
if (point_vector.size() < 4) { break; }
top0 = cv::Vec3d(point_vector[0].x, point_vector[0].y, 1);
bottom0 = cv::Vec3d(point_vector[1].x, point_vector[1].y, 1);
pt0 = cal3dXYZ(v_x, v_y, v_z, top0, bottom0, origin, scale_z, Hxy);
pt1 = cal3dXYZ(v_x, v_y, v_z, bottom0, bottom0, origin, scale_z, Hxy);
top1 = cv::Vec3d(point_vector[2].x, point_vector[2].y, 1);
bottom1 = cv::Vec3d(point_vector[3].x, point_vector[3].y, 1);
pt3 = cal3dXYZ(v_x, v_y, v_z, top1, bottom1, origin, scale_z, Hxy);
pt2 = cal3dXYZ(v_x, v_y, v_z, bottom1, bottom1, origin, scale_z, Hxy);
std::cout << "3D coordinate 0: " << pt0 << "\n";
std::cout << "3D coordinate 1: " << pt1 << "\n";
std::cout << "3D coordinate 2: " << pt2 << "\n";
std::cout << "3D coordinate 3: " << pt3 << "\n";
double text_w = distance3d(pt0, pt3);
double text_h = distance3d(pt0, pt1);
std::cout << "Save texture...\n";
src_text_vector.clear();
target_text_vector.clear();
src_text_vector.push_back(top0);
src_text_vector.push_back(bottom0);
src_text_vector.push_back(bottom1);
src_text_vector.push_back(top1);
target_text_vector.push_back(cv::Vec3d(0, 0, 1));
target_text_vector.push_back(cv::Vec3d(0, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, 0, 1));
cv::Mat text = getHomo(target_text_vector, src_text_vector);
std::cout << "texture: " << text_w << ", " << text_h << "\n";
cv::Mat texture(text_h, text_w, src.type(), Scalar(0, 0, 0));
getTexture(texture, src, text);
std::string text_file = std::to_string(cnt) + ".jpg";
cv::imwrite(text_file, texture);
cnt++;
std::string textures[] = { text_file };
std::cout << textures[0] << std::endl;
std::vector<cv::Vec3d> text_points;
text_points.push_back(pt1);
text_points.push_back(pt0);
text_points.push_back(pt3);
text_points.push_back(pt2);
std::ofstream fout("livingroom.wrl", std::ofstream::out | std::ofstream::app);
create_crml_file(text_points, fout, textures);
fout.close();
point_vector.clear();
}
else if (c == 'v')
{
if (point_vector.size() < 8) { break; }
top0 = cv::Vec3d(point_vector[0].x, point_vector[0].y, 1);
bottom0 = cv::Vec3d(point_vector[1].x, point_vector[1].y, 1);
pt0 = cal3dXYZ(v_x, v_y, v_z, top0, bottom0, origin, scale_z, Hxy);
top1 = cv::Vec3d(point_vector[2].x, point_vector[2].y, 1);
bottom1 = cv::Vec3d(point_vector[3].x, point_vector[3].y, 1);
pt1 = cal3dXYZ(v_x, v_y, v_z, top1, bottom1, origin, scale_z, Hxy);
top2 = cv::Vec3d(point_vector[4].x, point_vector[4].y, 1);
bottom2 = cv::Vec3d(point_vector[5].x, point_vector[5].y, 1);
pt2 = cal3dXYZ(v_x, v_y, v_z, top2, bottom2, origin, scale_z, Hxy);
top3 = cv::Vec3d(point_vector[6].x, point_vector[6].y, 1);
bottom3 = cv::Vec3d(point_vector[7].x, point_vector[7].y, 1);
pt3 = cal3dXYZ(v_x, v_y, v_z, top3, bottom3, origin, scale_z, Hxy);
std::cout << "3D coordinate 0: " << pt0 << "\n";
std::cout << "3D coordinate 1: " << pt1 << "\n";
std::cout << "3D coordinate 2: " << pt2 << "\n";
std::cout << "3D coordinate 3: " << pt3 << "\n";
double text_w = distance3d(pt0, pt3);
double text_h = distance3d(pt0, pt1);
std::cout << "Save texture...\n";
src_text_vector.clear();
target_text_vector.clear();
src_text_vector.push_back(top0);
src_text_vector.push_back(top1);
src_text_vector.push_back(top2);
src_text_vector.push_back(top3);
target_text_vector.push_back(cv::Vec3d(0, 0, 1));
target_text_vector.push_back(cv::Vec3d(0, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, 0, 1));
cv::Mat text = getHomo(target_text_vector, src_text_vector);
std::cout << "texture: " << text_w << ", " << text_h << "\n";
cv::Mat texture(text_h, text_w, src.type(), Scalar(0, 0, 0));
getTexture(texture, src, text);
std::string text_file = std::to_string(cnt) + ".jpg";
cv::imwrite(text_file, texture);
cnt++;
std::string textures[] = { text_file };
std::cout << textures[0] << std::endl;
std::vector<cv::Vec3d> text_points;
text_points.push_back(pt1);
text_points.push_back(pt0);
text_points.push_back(pt3);
text_points.push_back(pt2);
std::ofstream fout("livingroom.wrl", std::ofstream::out | std::ofstream::app);
create_crml_file(text_points, fout, textures);
fout.close();
point_vector.clear();
}
}
cvReleaseImage(&img);
cvReleaseImage(&image);
cvReleaseImage(&temp);
cvDestroyAllWindows();
return 0;
}
//----------- Begin of function LinAlg::quadratic_prog ------//
//! Interior Point Quadratic Programming
//! c, Q, A, b, xNames (no global or member variable access)
//!
//! try, by BM:
//! c = { 0,0 }
//! Q = { {2,0}, {0,5} }
//! A = { {5,6} }
//! b = { 10 }
//! xNames={x1,x2}
//!
bool LinearAlgebra::quadratic_prog(const Vector &v_c, const Matrix &m_Q, const Matrix &m_A, const Vector &v_b, Vector &v_xNames, int loopCountMultiplier) {
// init local variables
int maxIteration = 20;
const REAL SIGMA = 1/15.0; // 1/10.0;
const REAL R = 9.0/10;
int iter = 0;
int n = v_c.Storage();
int m = v_b.Storage();
maxIteration *= loopCountMultiplier;
#ifdef DEBUG_VC
DEBUG_LOG("----------- quadratic_prog begin -----------");
// print_input(c,Q,A,b,xNames)
if ( n==10 && m==12 ) {
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9), v_c(10));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f},",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f,",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12)
);
DEBUG_LOG(s);
/*sprintf(s, "%f, %f, %f, %f, %f, %f, %f, %f, %f,",
v_b(10),v_b(11),v_b(12),
v_b(13),v_b(14),v_b(15),
v_b(16),v_b(17),v_b(18)
);
DEBUG_LOG(s);
sprintf(s, "%f, %f};",
v_b(19),v_b(20));
DEBUG_LOG(s);*/
}
else if ( n==9 && m==20 ) { // stage 1
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, },",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f },",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, }, ",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20)
);
DEBUG_LOG(s);
}
else if ( n==10 && m==22 ) { // stage 2
char s[500];
DEBUG_LOG("c = ");
sprintf(s, "{ %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f };",
v_c(1),v_c(2),v_c(3),
v_c(4),v_c(5),v_c(6),
v_c(7),v_c(8),v_c(9),v_c(10));
DEBUG_LOG(s);
DEBUG_LOG("Q = ");
for (int i=1; i<=n; i++) {
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, },",
m_Q(i,1),m_Q(i,2),m_Q(i,3),m_Q(i,4),m_Q(i,5),m_Q(i,6),m_Q(i,7),m_Q(i,8),m_Q(i,9),m_Q(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("A = ");
for (i=1; i<=m; i++) {
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f },",
m_A(i,1),m_A(i,2),m_A(i,3),m_A(i,4),m_A(i,5),m_A(i,6),m_A(i,7),m_A(i,8),m_A(i,9),m_A(i,10)
);
DEBUG_LOG(s);
}
DEBUG_LOG("b = ");
sprintf(s, "{%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f, }, ",
v_b(1),v_b(2),v_b(3),
v_b(4),v_b(5),v_b(6),
v_b(7),v_b(8),v_b(9), v_b(10), v_b(11), v_b(12),
v_b(13),v_b(14),v_b(15), v_b(16), v_b(17), v_b(18), v_b(19), v_b(20), v_b(21), v_b(22)
);
DEBUG_LOG(s);
}
#endif
Vector v_eN(n); v_eN = 1;
Vector v_eM(m); v_eM = 1;
Vector v_x(n); v_x = 1;
Vector v_z(n); v_z = 1;
Vector v_y(m); v_y = 1;
Vector v_w(m); v_w = 1;
// check m_A is nxm, m_Q is nxn
err_when(m_A.Nrows() != m || m_A.Ncols() != n );
err_when(m_Q.Nrows() != n || m_Q.Ncols() != n );
// init for while loop not_converged() checking
//
Vector v_Road = v_b - m_A*(v_x) + v_w;
Vector v_Sigma = v_c - m_A.t() * v_y - v_z + m_Q*v_x;
REAL gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
REAL mute = SIGMA*gamma / (n+m);
DiagonalMatrix X(n), Xinv(n);
DiagonalMatrix Y(m), Yinv(m);
DiagonalMatrix Z(n);
DiagonalMatrix W(m);
Matrix T1(n,n), T2(n,m), T3(m,n), T4(m,m);
Vector KKFvec(n+m); //, v_tmp1(n), v_tmp2(m);
Vector m_temp(m+n);
Vector v_dx(n), v_dy(m), v_dz(n), v_dw(m);
REAL theeta;
X=0; Xinv=0; Z=0;
Y=0; Yinv=0; W=0; {
// Matrix _t1(n,n), _t2(n,n); _t1=1; _t2=2;
// _t1 = SP(_t1,_t2);
}
Matrix KKFmat(m+n,m+n);
Matrix KKFmatInverse(m+n,m+n);
REAL min=1;
while ( quadratic_prog_not_converged(v_x,v_y,v_Road,v_Sigma,gamma) && iter <= maxIteration && min > 0.0000000001 ) {
iter++;
v_Road = v_b - m_A*v_x + v_w;
v_Sigma = v_c - m_A.t()*v_y - v_z + m_Q*v_x;
gamma = (v_z.t()*v_x + v_y.t()*v_w).AsScalar();
mute = SIGMA*gamma / (n+m);
X.set_diagonal(v_x);
Y.set_diagonal(v_y);
Z.set_diagonal(v_z);
W.set_diagonal(v_w);
Xinv.set_diagonal(ElmDivide(v_eN, v_x));
Yinv.set_diagonal(ElmDivide(v_eM, v_y));
T1 = -(Xinv * Z + m_Q);
T2 = m_A.t();
T3 = m_A;
T4 = Yinv*W;
//PRT_MAT << "T1: " << T1 << endl;
KKFmat = (T1|T2) & (T3|T4);
KKFvec = (v_c - T2*v_y - mute*Xinv*v_eN + m_Q*v_x)
& (v_b - m_A*v_x + mute*Yinv*v_eM);
Try {
/*{
bool _f;
DiagonalMatrix _dmat(n+m); _dmat=1;
Matrix _inv(n+m,n+m);
SVD(KKFmat, _dmat, _inv);
_dmat=1;
if ( KKFmat*_inv == _dmat )
_f = true;
else
_f = false;
}*/
min = fabs(KKFmat(1,1));
for (int i=2; i<=m+n; i++) {
if ( min > fabs(KKFmat(i,i)) ) // check diagonal element
min = fabs(KKFmat(i,i));
}
KKFmatInverse = KKFmat.i();
/*
if ( KKFmat.LogDeterminant().Value() )
KKFmatInverse = KKFmat.i();
else
break;
*/
m_temp = KKFmatInverse * KKFvec;
v_dx = m_temp.Rows(1,n);
v_dy = m_temp.Rows(n+1,n+m);
v_dz = Xinv*(mute*v_eN - X*Z*v_eN - Z*v_dx);
v_dw = Yinv*(mute*v_eM - Y*W*v_eM - W*v_dy);
//PRT_MAT << "v_dx,z,y,w: " << (v_dx|v_dz) <<", "<< (v_dy|v_dw) <<endl;
//PRT_MAT << "v_x: " << v_x << endl;
//PRT_MAT << "ElmDivide(v_x,v_dx): " << ElmDivide(v_x,v_dx) << endl;
theeta =
(R/((
ElmDivide(-v_dx,v_x)
& ElmDivide(-v_dw,v_w)
& ElmDivide(-v_dy,v_y)
& ElmDivide(-v_dz,v_z)
)).MaximumValue()
);
if(theeta>1) // theeta = min(theeta,1)
theeta = 1;
v_x += theeta * v_dx;
v_y += theeta * v_dy;
v_w += theeta * v_dw;
v_z += theeta * v_dz;
#ifdef DEBUG_CONSOLE
cout << "#iter "<< iter << ": " << v_Road.Sum() << ", " << v_Sigma.Sum() << ", " << gamma << endl;
cout << "Ro:" << v_Road << endl;
PRT_MAT15 << "v_x:" << v_x << endl;
PRT_MAT15 << "v_y:" << v_y << endl;
PRT_MAT15 << "v_w:" << v_w << endl;
PRT_MAT15 << "v_z:" << v_z << endl;
#elif defined(DEBUG_VC)
char s[200];
sprintf(s, "#iter %d: %f, %f, %f",
iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
DEBUG_LOG(s);
if ( n == 9 && false ) {
DEBUG_LOG("x = ");
sprintf(s, " %f, %f, %f, %f, %f, %f, %f, %f, %f",
v_x(1),v_x(2),v_x(3),
v_x(4),v_x(5),v_x(6),
v_x(7),v_x(8),v_x(9));
DEBUG_LOG(s);
}
#endif
}
CatchAll {
#ifdef DEBUG_CONSOLE
cout << Exception::what();
#endif
#ifdef DEBUG_VC
DEBUG_LOG("olinalg: failure in quad_prog");
DEBUG_LOG((char *)Exception::what());
#endif
return false;
}
} // while
v_xNames = v_x;
if ( min < 0.00001 ) { // 1214 || KKFmat.LogDeterminant().Value() == 0 )
//char s[200];
//sprintf(s, "#iter %d: %f, %f, %f",
// iter, v_Road.Sum(), v_Sigma.Sum(), gamma);
//DEBUG_LOG(s);
DEBUG_LOG("--- quad_prog early exit for min < 0.00001 ---");
}
else
DEBUG_LOG("----------- quad_prog normal exit -----------");
#ifdef DEBUG_CONSOLE
cout << "#iter: " << iter;
#endif
return true;
}