//Add a wall plane facing in a direction void AddWall(scene& scene1, vector3 pos, vector3 dir, rgbf col =rgbf(0, 0.5, 0.5)) { plane_object* plane1 = new plane_object(pos, vector3(0, 0, 1)^dir); plane1->natrual_colour = col; plane1->k_spec = 0; scene1.add_object(plane1); }
void AddBox(scene& scene1, vector3 pos, float r =0.5f, rgbf col =rgbf(0, 0, 1), bool ref =false) { box* box1 = new box(pos, vector3(-r, -r, -r), vector3(r, r, r)); box1->natrual_colour = col; box1->reflective = ref; scene1.add_object(box1); }
//Add a sphere void AddSphere(scene& scene1, vector3 pos, float r =0.5f, rgbf col =rgbf(0, 0, 1), bool ref =false) { sphere_object* sphere = new sphere_object(r, pos); sphere->natrual_colour = col; sphere->reflective = ref; scene1.add_object(sphere); }
//Add a ceiling void AddCeiling(scene& scene1, vector3 pos, rgbf col =rgbf(0, 1, 0)) { plane_object* plane1 = new plane_object(pos, vector3(0, 0, -1)); plane1->natrual_colour = col; plane1->k_spec = 0; //No specular on floors scene1.add_object(plane1); }
void AddLightBall(scene& scene1, vector3 pos, int number, float r) { for (int i = 0; i < number; i++) { float t = 2 * M_PI / number * i; light* lig = new light(pos + vector3(r * cos(t), r * sin(t), 0), rgbf(1, 1, 1) , 1.0 / number); scene1.add_light(lig); } }
//Add triangle to mesh with normal void ATTMWN(mesh* m, vector3 v1, vector3 v2, vector3 v3, vector3 n) { triangle_object* tri = new triangle_object(v1, v1 - v1, v2 - v1, v3 - v1, n); tri->natrual_colour = rgbf(1, 1, 1); m->add_triangle(tri); }
void AddTriangleToMesh(mesh* m, vector3 v1, vector3 v2, vector3 v3) { triangle_object* tri = new triangle_object(v1, v1 - v1, v2 - v1, v3 - v1); tri->natrual_colour = rgbf(1, 1, 1); m->add_triangle(tri); }
void AddTriangle(scene& scene1, vector3 v1, vector3 v2, vector3 v3) { triangle_object* tri = new triangle_object(v1, v1 - v1, v2 - v1, v3 - v1); tri->natrual_colour = rgbf(1, 1, 1); scene1.add_object(tri); }
void TestFunction1(scene& scene1) { AddSphere(scene1, vector3(1.5, -1.5, 0), 0.5, rgbf(0, 0, 1), false); //AddSphere(scene1, vector3(1, 1, 0), 0.5, rgbf(0, 1, 1), true); sphere_object* s1 = new sphere_object(0.5, vector3(1.5, -1, 0)); s1->natrual_colour = rgbf(0, 0, 0); s1->reflective = true; s1->I_refl = 1; s1->k_spec = 1; s1->ambient_colour = rgbf(0, 0, 0); s1->shininess = 100; //scene1.add_object(s1); sphere_object* s2 = new sphere_object(0.5, vector3(1.5, 1, 0)); s2->transparent = true; s2->transparency = 0.9; s2->refindex = 0.9; scene1.add_object(s2); rgbf red(1, 0, 0); rgbf green(0, 1, 0); rgbf blue(0, 0, 1); rgbf yellow = red + green; rgbf magenta = red + blue; rgbf cyan = green + blue; AddWall(scene1, vector3(3, 0, -0.5), vector3(0, 1, 0), green * 0.7); AddWall(scene1, vector3(0, 3, -0.5), vector3(-1, 0, 0), red* 0.7); AddWall(scene1, vector3(-3, 0, -0.5), vector3(0, -1, 0), blue* 0.7); AddWall(scene1, vector3(0, -3, -0.5), vector3(1, 0, 0), yellow* 0.7); AddCeiling(scene1, vector3(0, 0, +3.5), magenta * 0.7); AddFloor(scene1, vector3(0, 0, -0.5), cyan * 0.7); //AddBox(scene1, vector3(1, -0.5, -0.3), 0.2, rgbf(1, 0, 1), true); box* box1 = new box(vector3(1, -0.5, 0), vector3(-0.2, -0.2, -0.2), vector3(0.2, 0.2, 0.2)); box1->natrual_colour = rgbf(0, 0, 0); box1->reflective = false; box1->transparent = true; box1->transparency = 1; box1->I_refr = 1; box1->refindex = 2; scene1.add_object(box1); //AddLightBall(scene1, vector3(-0.8, 0, 3), 3, 0.1); AddLight(scene1, vector3(-1.5, 1.5, 3)); //AddLight(scene1, vector3(2, 0, 4)); //AddLight(scene1, vector3(-5, 3, 2)); //IcoSphere(scene1, vector3(4, 0, 2)); torus_object* tor1 = new torus_object(0.1,0.5,vector3(1,0,-0.5)); tor1->natrual_colour = rgbf(1, 0, 0); //tor1->ambient_colour = rgbf(0, 0, 0.5); //scene1.add_object(tor1); mesh* m1 = ReadMesh("teapot.obj", vector3(1, -0.25, -0.5)); m1->natrual_colour = rgbf(0, 0, 0); m1->reflective = false; m1->transparent = true; m1->transparency = 1; m1->I_refr = 1; //scene1.add_object(m1); }
//Add a point light source void AddLight(scene& scene1, vector3 pos) { scene1.add_light(new light(pos, rgbf(1, 1, 1), 1.0f)); }
// *************************************************************************** void CMeshMorpher::update (std::vector<CAnimatedMorph> *pBSFactor) { uint32 i, j; if (_VBOri == NULL) return; if (BlendShapes.size() == 0) return; if (_VBOri->getNumVertices() != _VBDst->getNumVertices()) { // Because the original vertex buffer is not initialized by default // we must init it here (if there are some blendshapes) *_VBOri = *_VBDst; } // Does the flags are reserved ? if (_Flags.size() != _VBOri->getNumVertices()) { _Flags.resize (_VBOri->getNumVertices()); for (i = 0; i < _Flags.size(); ++i) _Flags[i] = Modified; // Modified to update all } nlassert(_VBOri->getVertexFormat() == _VBDst->getVertexFormat()); // Cleaning with original vertex buffer uint32 VBVertexSize = _VBOri->getVertexSize(); CVertexBufferRead srcvba; _VBOri->lock (srcvba); CVertexBufferReadWrite dstvba; _VBDst->lock (dstvba); const uint8 *pOri = (const uint8*)srcvba.getVertexCoordPointer (); uint8 *pDst = (uint8*)dstvba.getVertexCoordPointer (); for (i= 0; i < _Flags.size(); ++i) if (_Flags[i] >= Modified) { _Flags[i] = OriginalVBDst; for(j = 0; j < VBVertexSize; ++j) pDst[j+i*VBVertexSize] = pOri[j+i*VBVertexSize]; } uint tgSpaceStage = 0; if (_UseTgSpace) { tgSpaceStage = _VBDst->getNumTexCoordUsed() - 1; } // Blending with blendshape for (i = 0; i < BlendShapes.size(); ++i) { CBlendShape &rBS = BlendShapes[i]; float rFactor = pBSFactor->operator[](i).getFactor()/100.0f; // todo hulud check it works // if (rFactor > 0.0f) if (rFactor != 0.0f) for (j = 0; j < rBS.VertRefs.size(); ++j) { uint32 vp = rBS.VertRefs[j]; // Modify Pos/Norm/TgSpace. //------------ if (_VBDst->getVertexFormat() & CVertexBuffer::PositionFlag) if (rBS.deltaPos.size() > 0) { CVector *pV = dstvba.getVertexCoordPointer (vp); *pV += rBS.deltaPos[j] * rFactor; } if (_VBDst->getVertexFormat() & CVertexBuffer::NormalFlag) if (rBS.deltaNorm.size() > 0) { CVector *pV = dstvba.getNormalCoordPointer (vp); *pV += rBS.deltaNorm[j] * rFactor; } if (_UseTgSpace) if (rBS.deltaTgSpace.size() > 0) { CVector *pV = (CVector*)dstvba.getTexCoordPointer (vp, tgSpaceStage); *pV += rBS.deltaTgSpace[j] * rFactor; } // Modify UV0 / Color //------------ if (_VBDst->getVertexFormat() & CVertexBuffer::TexCoord0Flag) if (rBS.deltaUV.size() > 0) { CUV *pUV = dstvba.getTexCoordPointer (vp); *pUV += rBS.deltaUV[j] * rFactor; } if (_VBDst->getVertexFormat() & CVertexBuffer::PrimaryColorFlag) if (rBS.deltaCol.size() > 0) { // todo hulud d3d vertex color RGBA / BGRA CRGBA *pRGBA = (CRGBA*)dstvba.getColorPointer (vp); CRGBAF rgbf(*pRGBA); rgbf.R += rBS.deltaCol[j].R * rFactor; rgbf.G += rBS.deltaCol[j].G * rFactor; rgbf.B += rBS.deltaCol[j].B * rFactor; rgbf.A += rBS.deltaCol[j].A * rFactor; clamp(rgbf.R, 0.0f, 1.0f); clamp(rgbf.G, 0.0f, 1.0f); clamp(rgbf.B, 0.0f, 1.0f); clamp(rgbf.A, 0.0f, 1.0f); *pRGBA = rgbf; } // Modified _Flags[vp] = Modified; } } }
void demo::makemap_ICP(){ ros::Time start, end; Eigen::Matrix4d T_backup; T_backup.setIdentity(); PCloud cloud_s; PCloud cloud_d; //take 1 frame cloud_s = getframe(); pub_pointcloud.publish(cloud_s); std::cout<< 0 << "\n"; *cloud_key = *cloud_s; cloud_d = getframe(); ICP icp(cloud_s,cloud_d); RGBFeaturesInit rgbf(cloud_s,cloud_d); rgbf_key.setRansacThreshold(th); rgbf_key.setDesiredP(prob); rgbf.setRansacThreshold(th); rgbf.setDesiredP(prob); icp.setRThreshold (THRESH_dr); icp.setTThreshold (THRESH_dt); icp.setDisThreshold(D); icp.setNiterations (max_iterations); icp.setMaxPairs (MAXPAIRS); icp.setMinPairs (MINPAIRS); Eigen::Vector3d p1(0.0,0.0,0.0); Eigen::Vector3d p2(0.0,0.0,0.1); camera_positions.push_back(std::make_pair(p1, p2)); int i=1; float a1,a2; int ft; while(ros::ok()) { ROS_INFO("%d ", i); start = ros::Time::now(); ft = rgbf.compute(); end = ros::Time::now(); a1 = (end-start).toSec(); start = ros::Time::now(); if ( ft > f_th ){ icp.align(rgbf.T,rgbf.getInliers()); }else{ icp.align(T_backup); } end = ros::Time::now(); a2 = (end-start).toSec(); //update transformation T_backup=icp.returnTr(); T = T*icp.returnTr(); ROS_INFO("RGB: %.6f seconds | ICP: %.6f seconds | inliears= %d", a1, a2,ft); //update camera position p1 = T.block<3,3>(0,0)*camera_positions[0].first + T.block<3,1>(0,3); p2 = T.block<3,3>(0,0)*camera_positions[0].second + T.block<3,1>(0,3); camera_positions.push_back(std::make_pair(p1, p2)); if(checkIfIsNewKeyframe(cloud_d)){ ApplyRTto(cloud_d); pub_pointcloud.publish(cloud_d); } cloud_d = getframe(); //update icp clouds rgbf.setNewinputCloud(cloud_d); icp.setNewICPinputCloud(cloud_d); i++; } WriteCameraPLY(cameras_filename.data(), camera_positions); }