int main() { box x(point3d(1,2,3),point3d(4,5,6),color(234,4,56),"Box 1"); x.print_on(std::cout); return 0; }
void CMRGLView::TestOctomap() { cout << "[Octomap Test]" << endl; cout << endl; cout << "generating example map" << endl; OcTree tree(0.1); // create empty tree with resolution 0.1 // insert some measurements of occupied cells for (int x = -20; x<20; x++) { for (int y = -20; y<20; y++) { for (int z = -20; z<20; z++) { point3d endpoint((float)x*0.05f, (float)y*0.05f, (float)z*0.05f); tree.updateNode(endpoint, true); // integrate 'occupied' measurement } } } // insert some measurements of free cells for (int x = -30; x<30; x++) { for (int y = -30; y<30; y++) { for (int z = -30; z<30; z++) { point3d endpoint((float)x*0.02f - 1.0f, (float)y*0.02f - 1.0f, (float)z*0.02f - 1.0f); tree.updateNode(endpoint, false); // integrate 'free' measurement } } } cout << endl; cout << "performing some queries:" << endl; point3d query(0., 0., 0.); OcTreeNode* result = tree.search(query); print_query_info(query, result); query = point3d(-1., -1., -1.); result = tree.search(query); print_query_info(query, result); query = point3d(1., 1., 1.); result = tree.search(query); print_query_info(query, result); cout << endl; tree.writeBinary("simple_tree.bt"); cout << "wrote example file simple_tree.bt" << endl << endl; cout << "now you can use octovis to visualize: octovis simple_tree.bt" << endl; cout << "Hint: hit 'F'-key in viewer to see the freespace" << endl << endl; }
void create_serpinsky_pyramid(Scene * scene, int level) { Float pyramid_edge = 200; Float dx = -100; Float dy = -40; add_serpinsky_pyramid(scene, level, point3d(-pyramid_edge/2 + dx, -pyramid_edge * 0.87 / 2 + dy, 0), point3d(pyramid_edge/2 + dx, -pyramid_edge * 0.87 / 2 + dy, 0), point3d(dx, pyramid_edge * 0.87 / 2 + dy, 0), point3d(dx, dy, pyramid_edge * 0.87), material(1, 5, 0, 0, 0, 0), rgb(240, 210, 40)); }
int main() { fixed_array<double> point3d(3); point3d[0] = 2.3; point3d[1] = 3.2; point3d[2] = 4.2; for(fixed_array<double>::iterator i = point3d.begin(); i != point3d.end(); i++) { std::cout << *i << " "; } std::cout << std::endl; std::vector<double> vec; std::copy(point3d.begin(), point3d.end(), std::back_inserter(vec)); for(std::vector<double>::iterator i = vec.begin(); i != vec.end(); i++) { std::cout << *i << " "; } std::cout << std::endl; return 0; }
bool Reconstruction::initPoint(const track &t,point3d &p) { // projection matrices Matrix P1 = P_total[t.first_frame]; Matrix P2 = P_total[t.last_frame]; // observations point2d p1 = t.pixels.front(); point2d p2 = t.pixels.back(); // triangulation via orthogonal regression Matrix J(4,4); Matrix U,S,V; for (int32_t j=0; j<4; j++) { J.val[0][j] = P1.val[2][j]*p1.u - P1.val[0][j]; J.val[1][j] = P1.val[2][j]*p1.v - P1.val[1][j]; J.val[2][j] = P2.val[2][j]*p2.u - P2.val[0][j]; J.val[3][j] = P2.val[2][j]*p2.v - P2.val[1][j]; } J.svd(U,S,V); // return false if this point is at infinity float w = V.val[3][3]; if (fabs(w)<1e-10) return false; // return 3d point p = point3d(V.val[0][3]/w,V.val[1][3]/w,V.val[2][3]/w); return true; }
void line3d(SDL_Renderer *r, int x0, int y0, int z0, int x1, int y1, int z1) { int dx_A = abs(x1-x0), sx_A = x0<x1 ? 1 : -1; int dy_A = abs(y1-y0), sy_A = y0<y1 ? 1 : -1; int err_A = (dx_A>dy_A ? dx_A : -dy_A)/2, te_A; int dx_B = abs(z1-z0), sx_B = z0<z1 ? 1 : -1; int dy_B = abs(y1-y0), sy_B = y0<y1 ? 1 : -1; int err_B = (dx_B>dy_B ? dx_B : -dy_B)/2, te_B; int y0_A = y0, y0_B = y0; int done = 0; while(!done) { //Z loop for(;;){ if (z0==z1 && y0_B==y1) { done = 1; break; } te_B = err_B; if (te_B >-dx_B) { err_B -= dy_B; z0 += sx_B;} if (te_B < dy_B) { err_B += dx_B; y0_B += sy_B; break;} } //X loop for(;;){ point3d(r, x0, y0_A, z0); if (x0==x1 && y0_A==y1) break; te_A = err_A; if (te_A >-dx_A) { err_A -= dy_A; x0 += sx_A; } if (te_A < dy_A) { err_A += dx_A; y0_A += sy_A; break;} } } }
void ReferenceForwardIteratorTest::test_features() { FixedArray<double> point3d(3); point3d[0] = 2.3; point3d[1] = 3.2; point3d[2] = 4.2; for(FixedArray<double>::iterator i = point3d.begin(); i != point3d.end(); i++) { std::cout << *i << " "; } std::cout << std::endl; std::vector<double> vec; std::copy(point3d.begin(), point3d.end(), std::back_inserter(vec)); for(std::vector<double>::iterator i = vec.begin(); i != vec.end(); i++) { std::cout << *i << " "; } std::cout << std::endl; }
void cal_cube(int size){ int i; for(i=0;i<8;i++){ cube1.p[i]=point3d(size*dirx[i],size*diry[i],size*dirz[i]); } }
InfoNode::InfoNode() { infoGain = 0; cost = 0; quality = 0; coords = point3d(0, 0, 0); pitch = 0; yaw = 0; }
multi_array<cv::rgb, 2> gray_word(const multi_array<cv::rgb, 2> &img){ auto img_double = lazy_cast(img, point3d()); auto color_mean = mean(img_double); auto gray_mean = mean(color_mean); auto k_r = gray_mean / color_mean[0]; auto k_g = gray_mean / color_mean[1]; auto k_b = gray_mean / color_mean[2]; }
void generate_random_spheres(Scene * scene, int count) { srand(time(NULL)); int i; for(i = 0; i < count; i++) { int x = (rand() % 200) - (rand() % 200); int y = (rand() % 200) - (rand() % 200); int z = (rand() % 200) - (rand() % 200); int r = (rand() % 40); add_object(scene, new_sphere(point3d(x, y, z), r, rgb(255, 0, 0), material(1, 5, 0, 0, 0, 0))); } }
Camera * create_camera(void) { Point3d camera_location = point3d(-70, 200, 50); Float focus = 320; Float x_angle = -1.57; Float y_angle = 0; Float z_angle = 3.14; Camera * camera = new_camera(camera_location, x_angle, y_angle, z_angle, focus); return camera; }
//Do a standard 2D bresenham, but draw it so that 'y' values are plotted on z-axis //and plotted y-coordinate stays fixed at scanline void scanline3d(SDL_Renderer *r, int scanline, int x0, int z0, int x1, int z1) { int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1; int dz = abs(z1-z0), sz = z0<z1 ? 1 : -1; int err = (dx>dz ? dx : dz)/2, te; for(;;) { point3d(r, x0, scanline, z0); if(x0==x1 && z0 == z1) return; te = err; if(te > -dx) { err -= dz; x0 += sx; } if(te < dz) { err += dx; z0 += sz; } } }
void move_camera(Camera * const camera, const Vector3d vector) { Vector3d r_vector = rotate_vector_x(vector, camera->sin_al_x, camera->cos_al_x); r_vector = rotate_vector_z(r_vector, camera->sin_al_z, camera->cos_al_z); r_vector = rotate_vector_y(r_vector, camera->sin_al_y, camera->cos_al_y); Point3d curr_pos = camera->camera_position; camera->camera_position = point3d(curr_pos.x + r_vector.x, curr_pos.y + r_vector.y, curr_pos.z + r_vector.z); }
int main() { std::list<shape*> li; shape *a; srand(time(NULL)); int box_nr=0; int kugel_nr=0; for(int i=0;i<11;++i) { int x=rand()%2; if(x==0) { ++box_nr; std::stringstream sstr; std::string name ="Box "; std::string x; sstr << box_nr; sstr >> x; name=name.append(x); box* b=new box(point3d((rand()%10),(rand()%20)+11,(rand()%30)+21),point3d((rand()%30)+21,(rand()%10),(rand()%20)+11),color(i*10+i,i*11,i*i),name); a=b; } if(x==1) { ++kugel_nr; std::stringstream sstr; std::string name ="Kugel "; std::string x; sstr << kugel_nr; sstr >> x; name=name.append(x); sphere* b=new sphere(point3d((rand()%10),(rand()%10),(rand()%10)),rand()%10,color(i*10+i,i*11,i*i),name); a=b; }
/* * Request double buffer display mode. * Register mouse input callback functions */ int main(int argc, char** argv) { glutInit(&argc, argv); init (); cal_cube(100); zp=100; //draw_cube(cube1); cop=point3d(0,0,600); glutDisplayFunc(display); glutReshapeFunc(reshape); glutMouseFunc(mouse); glutSpecialFunc(specialDown); glutSpecialUpFunc(specialUp); glutIdleFunc(keyCheck); glutMainLoop(); return 0; }
void add_serpinsky_pyramid(Scene * scene, int depth, Point3d p1, Point3d p2, Point3d p3, Point3d p4, Material material, Color color) { if(depth) { Point3d p1p2 = point3d((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); Point3d p1p3 = point3d((p1.x + p3.x) / 2, (p1.y + p3.y) / 2, (p1.z + p3.z) / 2); Point3d p1p4 = point3d((p1.x + p4.x) / 2, (p1.y + p4.y) / 2, (p1.z + p4.z) / 2); Point3d p2p3 = point3d((p2.x + p3.x) / 2, (p2.y + p3.y) / 2, (p2.z + p3.z) / 2); Point3d p2p4 = point3d((p2.x + p4.x) / 2, (p2.y + p4.y) / 2, (p2.z + p4.z) / 2); Point3d p3p4 = point3d((p3.x + p4.x) / 2, (p3.y + p4.y) / 2, (p3.z + p4.z) / 2); add_serpinsky_pyramid(scene, depth - 1, p1, p1p2, p1p3, p1p4, material, color); add_serpinsky_pyramid(scene, depth - 1, p2, p1p2, p2p3, p2p4, material, color); add_serpinsky_pyramid(scene, depth - 1, p3, p1p3, p2p3, p3p4, material, color); add_serpinsky_pyramid(scene, depth - 1, p4, p1p4, p2p4, p3p4, material, color); } else { add_pyramid(scene, p1, p2, p3, p4, material, color); } }
bool world::render() { glutwindow& gw=glutwindow::instance(); ppmwriter pw(width_, heigth_, filename_); ray r; // Aus dem Öffnungswinkel der Abstand der Kamera berechnet // tan(90° - alpha/2) * breite/2 double cam_abstand=std::tan(((90.0 - camera_fov_) / 2)*(M_PI / 180)) * width_ * 0.5; r.ori=point3d(0.0, 0.0, 0.0); std::cout << "camera abstand: " << cam_abstand << std::endl; std::cout << "breite: " << width_ << std::endl; for (std::size_t y=0; y < heigth_; ++y) { for (std::size_t x=0; x < width_; ++x) { // Pixel pixel p(x, y); // Anti-Aliasing für jeden Pixel werden AA_RES^2 Rays generiert und die Farben aufsummiert und durch AA_RES geteilt rgb aa_color = bg_; for (std::size_t pp=0; pp < AA_RES; ++pp) for (std::size_t qq=0; qq < AA_RES; ++qq) { //Berechnung der Kamerafläche double ux=(y - (0.5 * (width_ - 1)) + (pp + 0.5) / AA_RES); double uy=(x - (0.5 * (heigth_ - 1)) + (qq + 0.5) / AA_RES); r.dir=vector3d(r.ori, point3d(ux, uy, -cam_abstand)); r.dir.normalize(); shade sh; master_.intersect(r, sh); if (sh.didhit) { aa_color+=beleucht_.color(r, sh); } else { aa_color+=bg_; } } p.color = aa_color; p.color /= AA_RES*AA_RES; //ausgeben und schreiben gw.write(p); pw.write(p); } } //Bild speichern pw.save(); std::cout << "end" << std::endl; return true; }
point3d shift_linear(float _x,float _y,float _z) {return point3d(x+_x,y+_y,z+_z);};
const double pixelPerCmHorizontal = 1000/26.8; const double pixelPerCmVertical = 1000/26.1; // Distance of eyes from screen (in Centimeters) const double distanceFromScreenCm = 60; // Distance of eyes above 0/0 coordinates (in Centimeters) const double distanceAboveCenterCm = 10; // Distance of eyes right of 0/0 coordinates (in Centimeters) const double distanceRightOfCenterCm = 10; // Distance of left eye from right eye const double distanceEyesCm = 6.8; const point3d left_eye = point3d(distanceRightOfCenterCm - distanceEyesCm/2, distanceAboveCenterCm, distanceFromScreenCm); const point3d right_eye = point3d(distanceRightOfCenterCm + distanceEyesCm/2, distanceAboveCenterCm, distanceFromScreenCm); point2d MapPointOntoScreen(point3d const& eye, point3d const& point) { point2d point_; point_.x = (point.x - eye.x) / (eye.z - point.z) * eye.z + eye.x; point_.y = (point.y - eye.y) / (eye.z - point.z) * eye.z + eye.y; return point_; }
// キャリブレーション __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); } }
sphere::sphere(double x, double y, double z, double r, material const& m) : center_(point3d(x,y,z)), radius_(r), shape(m) { bbox(); }
void Pointcloud::push_back(const Pointcloud& other) { for (Pointcloud::const_iterator it = other.begin(); it != other.end(); it++) { points.push_back(point3d(*it)); } }
point3d shift_sphere(float radius,float angle_h,float angle_v) {return point3d(x+cos(angle_h)*sin(angle_v)*radius,y+sin(angle_h)*sin(angle_v)*radius,z+cos(angle_v));};
LRESULT CALLBACK WindowProc( HWND hWnd, UINT Msg, WPARAM wParam, LPARAM lParam) { switch(Msg) { case WM_PAINT: { // Request to paint PAINTSTRUCT ps; HDC dc = BeginPaint(hWnd, &ps); SetROP2(dc, R2_MERGEPEN); DrawLine(dc, left_eye, right_eye, point3d(-4, -4, -4), point3d( 4, -4, -4)); DrawLine(dc, left_eye, right_eye, point3d(-4, -4, -4), point3d(-4, 4, -4)); DrawLine(dc, left_eye, right_eye, point3d(-4, -4, -4), point3d(-4, -4, 4)); DrawLine(dc, left_eye, right_eye, point3d(-4, 4, -4), point3d( 4, 4, -4)); DrawLine(dc, left_eye, right_eye, point3d( 4, 4, -4), point3d( 4, -4, -4)); DrawLine(dc, left_eye, right_eye, point3d( 4, -4, -4), point3d( 4, -4, 4)); DrawLine(dc, left_eye, right_eye, point3d( 4, -4, 4), point3d(-4, -4, 4)); DrawLine(dc, left_eye, right_eye, point3d( 4, -4, 4), point3d( 4, 4, 4)); DrawLine(dc, left_eye, right_eye, point3d( 4, 4, -4), point3d( 4, 4, 4)); DrawLine(dc, left_eye, right_eye, point3d(-4, 4, -4), point3d(-4, 4, 4)); DrawLine(dc, left_eye, right_eye, point3d(-4, 4, 4), point3d( 4, 4, 4)); DrawLine(dc, left_eye, right_eye, point3d(-4, 4, 4), point3d(-4, -4, 4)); EndPaint (hWnd, &ps); return 0; } case WM_DESTROY: // Sent when a window is being destroyed. PostQuitMessage(/* nExitCode: */ 0 /*WM_QUIT*/); // MSDN: If an application processes this message, // it should return zero: return 0; default: // Process the left-over messages return DefWindowProc(hWnd, Msg, wParam, lParam); } }
point3d shift_radial_h(float radius,float angle) {return point3d(x+cos(angle)*radius,y+sin(angle)*radius,z);};
Pointcloud::Pointcloud(Pointcloud* other) { for (Pointcloud::const_iterator it = other->begin(); it != other->end(); it++) { points.push_back(point3d(*it)); } }
point3d shift_radial_v(float radius,float angle) {return point3d(x+cos(angle)*radius,y,z+sin(angle)*radius);};
void CDX9Renderer::SmoothLine(point p1, point p2, cr_float width, const color& c) { SmoothLine3D(point3d(p1.x, p1.y, 0), point3d(p2.x, p2.y, 0), width, c); }
int main(void) { // Allocating scene Scene * scene = new_scene(MAX_OBJECTS_NUMBER, MAX_LIGHT_SOURCES_NUMBER, BACKGROUND_COLOR); // Allocating new sphere Float radius = 100; Point3d center = point3d(0, 0, 0); Color sphere_color = rgb(250, 30, 30); Material sphere_material = material(1, 5, 5, 10, 0, 10); Object3d * sphere = new_sphere(center, radius, sphere_color, sphere_material); // Adding sphere to the scene add_object(scene, sphere); // Allocating new triangle Object3d * triangle = new_triangle(point3d(-700, -700, -130), // vertex 1 point3d( 700, -700, -130), // vertex 2 point3d( 0, 400, -130), // vertex 3 rgb(100, 255, 30), // color material(1, 6, 0, 2, 0, 0) // surface params ); // Adding triangle to the scene add_object(scene, triangle); // Loading 3D model of cow from *.obj file // defining transformations and parameters of 3D model // TODO: must be refactored... SceneFaceHandlerParams load_params = new_scene_face_handler_params(scene, // scale: 40, // move dx, dy, dz: -150, -100, 30, // rotate around axises x, y, z: 0, 0, 0, // color rgb(200, 200, 50), // surface params material(2, 3, 0, 0, 0, 0) ); load_obj("./demo/models/cow.obj", // default handler which adding polygons of 3D model to scene: scene_face_handler, &load_params); // This function is requried (bulding k-d tree of entire scene) prepare_scene(scene); printf("\nNumber of polygons: %i\n", scene->last_object_index + 1); // Allocating new light source Color light_source_color = rgb(255, 255, 255); Point3d light_source_location = point3d(-300, 300, 300); LightSource3d * light_source = new_light_source(light_source_location, light_source_color); // Adding light source to the scene add_light_source(scene, light_source); // Adding fog Float density = 0.002; set_exponential_fog(scene, density); // Allocating camera // TODO: It's a pity, but quaternions are not implemented yet :( Point3d camera_location = point3d(0, 500, 0); Float focus = 320; Float x_angle = -1.57; Float y_angle = 0; Float z_angle = 3.14; Camera * camera = new_camera(camera_location, x_angle, y_angle, z_angle, focus); // Rotate camera if needed // rotate_camera(camera, d_x_angle, d_y_angle, d_z_angle); // Move camera if needed // move_camera(camera, vector3df(d_x, d_y, d_z)); // Alocate new canvas, to render scene on it Canvas * canvas = new_canvas(CANVAS_W, CANVAS_H); render_scene(scene, camera, canvas, THREADS_NUM); // Saving rendered image in PNG format write_png("example.png", canvas); release_canvas(canvas); release_scene(scene); release_camera(camera); return 0; }