LuftBallon

google::SetStderrLogging(google::INFO); // print the logs whose severity > [info]

#include <FreeImage.h>
#include <opencv2\opencv.hpp>
using namespace cv;

// #define _CRT_SECURE_NO_WARNINGS
#pragma warning(disable : 4996)
void FI2MAT(FIBITMAP* src, Mat& dst)
{
    //FIT_BITMAP    //standard image : 1 - , 4 - , 8 - , 16 - , 24 - , 32 - bit
    //FIT_UINT16    //array of unsigned short : unsigned 16 - bit
    //FIT_INT16     //array of short : signed 16 - bit
    //FIT_UINT32    //array of unsigned long : unsigned 32 - bit
    //FIT_INT32     //array of long : signed 32 - bit
    //FIT_FLOAT     //array of float : 32 - bit IEEE floating point
    //FIT_DOUBLE    //array of double : 64 - bit IEEE floating point
    //FIT_COMPLEX   //array of FICOMPLEX : 2 x 64 - bit IEEE floating point
    //FIT_RGB16     //48 - bit RGB image : 3 x 16 - bit
    //FIT_RGBA16    //64 - bit RGBA image : 4 x 16 - bit
    //FIT_RGBF      //96 - bit RGB float image : 3 x 32 - bit IEEE floating point
    //FIT_RGBAF     //128 - bit RGBA float image : 4 x 32 - bit IEEE floating point

    int bpp = FreeImage_GetBPP(src);
    FREE_IMAGE_TYPE fit = FreeImage_GetImageType(src);

    int cv_type = -1;
    int cv_cvt = -1;

    switch (fit)
    {
    case FIT_UINT16: cv_type = DataType<ushort>::type; break;
    case FIT_INT16: cv_type = DataType<short>::type; break;
    case FIT_UINT32: cv_type = DataType<unsigned>::type; break;
    case FIT_INT32: cv_type = DataType<int>::type; break;
    case FIT_FLOAT: cv_type = DataType<float>::type; break;
    case FIT_DOUBLE: cv_type = DataType<double>::type; break;
    case FIT_COMPLEX: cv_type = DataType<Complex<double>>::type; break;
    case FIT_RGB16: cv_type = DataType<Vec<ushort, 3>>::type; cv_cvt = COLOR_RGB2BGR; break;
    case FIT_RGBA16: cv_type = DataType<Vec<ushort, 4>>::type; cv_cvt = COLOR_RGBA2BGRA; break;
    case FIT_RGBF: cv_type = DataType<Vec<float, 3>>::type; cv_cvt = COLOR_RGB2BGR; break;
    case FIT_RGBAF: cv_type = DataType<Vec<float, 4>>::type; cv_cvt = COLOR_RGBA2BGRA; break;
    case FIT_BITMAP:
        switch (bpp) {
        case 8: cv_type = DataType<Vec<uchar, 1>>::type; break;
        case 16: cv_type = DataType<Vec<uchar, 2>>::type; break;
        case 24: cv_type = DataType<Vec<uchar, 3>>::type; break;
        case 32: cv_type = DataType<Vec<uchar, 4>>::type; break;
        default:
            // 1, 4 // Unsupported natively
            cv_type = -1;
        }
        break;
    default:
        // FIT_UNKNOWN // unknown type
        dst = Mat(); // return empty Mat
        return;
    }

    int width = FreeImage_GetWidth(src);
    int height = FreeImage_GetHeight(src);
    int step = FreeImage_GetPitch(src);

    if (cv_type >= 0) {
        dst = Mat(height, width, cv_type, FreeImage_GetBits(src), step);
        if (cv_cvt > 0)
        {
            cvtColor(dst, dst, cv_cvt);
        }
    }
    else {

        std::vector<uchar> lut;
        int n = pow(2, bpp);
        for (int i = 0; i < n; ++i)
        {
            lut.push_back(static_cast<uchar>((255 / (n - 1))*i));
        }

        FIBITMAP* palletized = FreeImage_ConvertTo8Bits(src);
        BYTE* data = FreeImage_GetBits(src);
        for (int r = 0; r < height; ++r) {
            for (int c = 0; c < width; ++c) {
                dst.at<uchar>(r, c) = saturate_cast<uchar>(lut[data[r*step + c]]);
            }
        }
    }

    flip(dst, dst, 0);
}

int main()
{
    FreeImage_Initialise();
    // std::string imgPath = "F:/cppTry/index3.png";
    std::string imgPath = "F:/cppTry/ger.JPG";
    FREE_IMAGE_FORMAT format = FreeImage_GetFileType(imgPath.c_str(), 0);
    FIBITMAP* fi_image = FreeImage_Load(format, imgPath.c_str());

    Mat cvImg2;
    FI2MAT(fi_image, cvImg2);
    cvNamedWindow("Image1:",1);
    cv::imshow("Image1:",cvImg2);
    // imshow(cv_img);

    IplImage *img = cvLoadImage(imgPath.c_str());
    cvNamedWindow("Image2:",1);
    cvShowImage("Image2:", img);
    std::cout << "[free's conv]: type is: "    << FreeImage_GetColorType(fi_image) << "\n";
    std::cout << "[free's conv]: depth is: "   << FreeImage_GetBPP(fi_image) << "\n";
    std::cout << "[free's conv]: pitch is: "   << FreeImage_GetPitch(fi_image) << "\n";
    std::cout << "[cv2 conv]: type is: " <<  "/" << cvImg2.type() << "\n";
    std::cout << "[cv2 conv]: depth is: " << "/" << cvImg2.depth() << "\n";
    std::cout << "[cv2 conv]: channel is: " << "/" << cvImg2.channels() << "\n";
    std::cout << "[cv2 conv]: elem size is: " << "/" << cvImg2.elemSize() << "\n";

    cvWaitKey();
    cvDestroyWindow("Image:");
    return 0;
}

void convert1to3Channel(cv::Mat& src, cv::Mat& dst) {
    vector<cv::Mat> channels;
    for (int i = 0; i < 3; i++) {
        channels.push_back(src);
    }
    cv::merge(&channels[0], channels.size(), dst);
}

void viewThread(pangolin::OpenGlMatrix &Twc) {
	pangolin::CreateWindowAndBind("Main", 1024, 768);
	glEnable(GL_DEPTH_TEST);
	// Issue specific OpenGl we might need
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    
    // Define Projection and initial ModelView matrix
	pangolin::OpenGlRenderState s_cam(
		pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
		pangolin::ModelViewLookAt(50, 50, 50, 0, 0, 0, pangolin::AxisY)
	);

	glClear(GL_COLOR_BUFFER_BIT);
	glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

	 // Add named OpenGL viewport to window and provide 3D Handler
	 pangolin::View& d_cam = pangolin::CreateDisplay()
	 	.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
	 	.SetHandler(new pangolin::Handler3D(s_cam));
	
	size_t frame = 0;
    while( !pangolin::ShouldQuit() ) {
        // Clear screen and activate view to render into
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glClearColor(1.0f,1.0f,1.0f,1.0f);
        //! Activate Displays and set State Matrices
        d_cam.Activate(s_cam);
		// draw camera
        const float w = 2;
        const float h = w*0.75;
        const float z = w*0.6;
        glPushMatrix();
		glMultMatrixd(Twc.m);
        glLineWidth(1);
        glColor3f(0.0f,1.0f,0.0f);
        glBegin(GL_LINES);
        glVertex3f(0,0,0);
        glVertex3f(w,h,z);
        glVertex3f(0,0,0);
        glVertex3f(w,-h,z);
        glVertex3f(0,0,0);
        glVertex3f(-w,-h,z);
        glVertex3f(0,0,0);
        glVertex3f(-w,h,z);

        glVertex3f(w,h,z);
        glVertex3f(w,-h,z);

        glVertex3f(-w,h,z);
        glVertex3f(-w,-h,z);

        glVertex3f(-w,h,z);
        glVertex3f(w,h,z);

        glVertex3f(-w,-h,z);
        glVertex3f(w,-h,z);
        glEnd();

        glPopMatrix();

		// draw points
		glPointSize(2);
		glBegin(GL_POINTS);
		glColor3f(0.0, 0.0, 0.0);
		
		// get all points to m_pos.m_ppatches
		updatePointsMutex.lock();
		for (size_t p = 0; p < (size_t)allPPatchesRender.size(); ++p)
		{
			auto patch = *allPPatchesRender[p];
			glVertex3f(
				patch.m_coord[0],
				patch.m_coord[1],
				patch.m_coord[2]
			);
		}
        updatePointsMutex.unlock();
        glEnd();

        // Swap frames and Process Events
        pangolin::FinishFrame();
        
		std::this_thread::sleep_for(std::chrono::milliseconds(100)); // ms
		// std::cout << "frame��" << ++frame << std::endl;
		// std::cout << "point nums are: " << findMatch->m_pos.m_ppatches.size() << std::endl;
    }
}


int main(int argc, char* argv[])
{
	pangolin::OpenGlMatrix Twc, Twr;
	Twc.SetIdentity();
	std::thread viewThd(viewThread, Twc);

    while(1) {
...<省略>
        // update render points
        updatePointsMutex.lock();
        allPPatchesRender = findMatch->m_pos.m_ppatches; // to get the newest points
        updatePointsMutex.unlock();
...<省略>
    }


	viewThd.join();

	//writePointCloundPly(prefix);
	releasePMVS();
	return 0;
}

class QOsgWidget {
public:
    ///< essential widget, use this ptr to be the real widget
    osgQOpenGLWidget* pWidget = nullptr;
    // QOsgWidget(QWidget* parent = nullptr);
    QOsgWidget(const std::string& modelPath, QWidget* parent = nullptr);
    ~QOsgWidget();

    ///< osg base vars
    osg::ref_ptr<osg::Group>                        mRoot                = nullptr; // root node of the osg scene
    osg::ref_ptr<osg::Camera>                       camera               = nullptr; // osg camera
    osg::ref_ptr<osgViewer::Viewer>                 mViewer              = nullptr; // osg viewer
    osg::ref_ptr<osgGA::TrackballManipulator>       trackball            = nullptr;
    osg::ref_ptr<osgGA::KeySwitchMatrixManipulator> keyswitchManipulator = nullptr;
    osg::ref_ptr<osgViewer::StatsHandler>           stats                = nullptr;
    <被省略>
}

QOsgWidget::~QOsgWidget() {
    // end the render thread and destroy the osgOpenglWidget
    std::this_thread::sleep_for(std::chrono::microseconds(1000));
    mViewer->setDone(true);
    std::this_thread::sleep_for(std::chrono::microseconds(1000));
    mViewer->stopThreading();

    //////////////////////////////////////////////////////////////////////////
    /// without folowing post process, u will crash with the bellowing warning when: delete pWidget
    /// Warning: deleting still referenced object 000002433CA59BB0 of type 'class osg::Referenced * __ptr64'
    ///     the final reference count was 1, memory corruption possible.
    ///
    /// the reason i guess is:
    /// the bellowing vars can not be automatically set to null when delete pWidget, because they does
    /// not belong to the pWidget, so u need to set them to null manually, otherwise when delete pWidget,
    /// the destructor of pWidget will try to free the bellowing vars, but their ref != 0
    ///////////////////////////////// PART 1 /////////////////////////////////
        mRoot = nullptr;
        mViewer = nullptr;
        camera = nullptr;
        trackball = nullptr;
        keyswitchManipulator = nullptr;
    //////////////////////////////////////////////////////////////////////////

    delete pWidget; // call the destructor of the osgOpenglWidge
}

            resize(height() * 639 / 480, height());
            QOsgWidget *bBoxEdit = new QOsgWidget(modelPath, static_cast<QWidget*>(this));
            // use an ArgumentParser object to manage the program arguments.
            bBoxEdit->pWidget = new osgQOpenGLWidget(&arguments, this);
            bBoxEdit->pWidget->show();

            // init the manipulators
            bBoxEdit->InitManipulators();
            bBoxEdit->InitModel(modelPath, bBoxEdit->mRoot);

// **************** part0: inside code segment of func call **********************/
            osg::ref_ptr<osgViewer::Viewer> mViewer = bBoxEdit->pWidget->getOsgViewer();
            // Add a Stats Handler to the viewer
            mViewer->addEventHandler(new osgViewer::StatsHandler);
            // Add the Camera to the Viewer
            osg::ref_ptr<osg::Camera> camera = mViewer->getCamera();
            // Set projection matrix and camera attribtues
            camera->setClearMask(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
            camera->setClearColor(osg::Vec3f(0.2f, 0.2f, 0.4f, 1.0f));
            //mViewer->addSlave(camera.get());
            mViewer->setCamera(camera.get());
            // Set the Scene Data

// *************************** part1: func call **********************************/
            // bBoxEdit->InitCameraConfig();

void QOsgWidget::InitCameraConfig() {
    // Create the viewer for this window
    mViewer = this->pWidget->getOsgViewer();
    // Add a Stats Handler to the viewer
    mViewer->addEventHandler(new osgViewer::StatsHandler);

    // Add the Camera to the Viewer
    osg::ref_ptr<osg::Camera> camera = mViewer->getCamera();
    // Set projection matrix and camera attribtues
    camera->setClearMask(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
    camera->setClearColor(osg::Vec3f(0.2f, 0.2f, 0.4f, 1.0f));
    //mViewer->addSlave(camera.get());
    mViewer->setCamera(camera.get());

    // Add the Camera Manipulator to the Viewer
    osg::ref_ptr<osgGA::TrackballManipulator> trackball = new osgGA::TrackballManipulator();
    mViewer->setCameraManipulator(trackball.get());

    // Set the Scene Data
    mViewer->setSceneData(mRoot.get());
}

#include <osgQOpenGL/osgQOpenGLWidget>

#include <osgDB/ReadFile>
#include <osgUtil/Optimizer>
#include <osg/CoordinateSystemNode>

#include <osg/Switch>
#include <osg/Types>
#include <osgViewer/Viewer>
#include <osgText/Text>

#include <osgViewer/Viewer>
#include <osgViewer/ViewerEventHandlers>

#include <osgGA/TrackballManipulator>
#include <osgGA/FlightManipulator>
#include <osgGA/DriveManipulator>
#include <osgGA/KeySwitchMatrixManipulator>
#include <osgGA/StateSetManipulator>
#include <osgGA/AnimationPathManipulator>
#include <osgGA/TerrainManipulator>
#include <osgGA/SphericalManipulator>

#include <osgGA/Device>

#include <QApplication>
#include <QSurfaceFormat>

#include <iostream>


int main( int argc, char** argv )
{


    QSurfaceFormat format = QSurfaceFormat::defaultFormat();

#ifdef OSG_GL3_AVAILABLE
    format.setVersion(3, 2);
    format.setProfile(QSurfaceFormat::CoreProfile);
    format.setRenderableType(QSurfaceFormat::OpenGL);
    format.setOption(QSurfaceFormat::DebugContext);
#else
    format.setVersion(2, 0);
    format.setProfile(QSurfaceFormat::CompatibilityProfile);
    format.setRenderableType(QSurfaceFormat::OpenGL);
    format.setOption(QSurfaceFormat::DebugContext);
#endif
    format.setDepthBufferSize(24);
    //format.setAlphaBufferSize(8);
    format.setSamples(8);
    format.setStencilBufferSize(8);
    format.setSwapBehavior(QSurfaceFormat::DoubleBuffer);
    QSurfaceFormat::setDefaultFormat(format);

    QApplication app(argc, argv);

    // use an ArgumentParser object to manage the program arguments.
    osg::ArgumentParser arguments(&argc, argv);

    arguments.getApplicationUsage()->setApplicationName(
        arguments.getApplicationName());
    arguments.getApplicationUsage()->setDescription(arguments.getApplicationName() +
                                                    " is the standard OpenSceneGraph example which loads and visualises 3d models.");
    arguments.getApplicationUsage()->setCommandLineUsage(
        arguments.getApplicationName() + " [options] filename ...");
    arguments.getApplicationUsage()->addCommandLineOption("--image <filename>",
                                                          "Load an image and render it on a quad");
    arguments.getApplicationUsage()->addCommandLineOption("--dem <filename>",
                                                          "Load an image/DEM and render it on a HeightField");
    arguments.getApplicationUsage()->addCommandLineOption("--login <url> <username> <password>",
                                                          "Provide authentication information for http file access.");
    arguments.getApplicationUsage()->addCommandLineOption("-p <filename>",
                                                          "Play specified camera path animation file, previously saved with 'z' key.", 
                                                          "F:/dataSets/OpenSceneGraph-Data-3.4.0/OpenSceneGraph-Data/glider.osg");
    arguments.getApplicationUsage()->addCommandLineOption("--speed <factor>",
                                                          "Speed factor for animation playing (1 == normal speed).");
    arguments.getApplicationUsage()->addCommandLineOption("--device <device-name>",
                                                          "add named device to the viewer");

    osgQOpenGLWidget widget(&arguments);

    if (true) {

        osg::ref_ptr<osg::Node> loadedModel = osgDB::readNodeFile("F:/dataSets/OpenSceneGraph-Data-3.4.0/OpenSceneGraph-Data/glider.osg");

        if (!loadedModel)
        {
            std::cout << arguments.getApplicationName() << ": No data loaded" << std::endl;
            return 1;
        }

        // any option left unread are converted into errors to write out later.
        arguments.reportRemainingOptionsAsUnrecognized();

        // report any errors if they have occurred when parsing the program arguments.
        if (arguments.errors())
        {
            arguments.writeErrorMessages(std::cout);
            return 1;
        }

        widget.show();

        // optimize the scene graph, remove redundant nodes and state etc.
        osgUtil::Optimizer optimizer;
        optimizer.optimize(loadedModel);

        // mViewer = new osgViewer::Viewer();
        osg::ref_ptr<osgViewer::Viewer> mViewer = widget.getOsgViewer();
       
    		osg::ref_ptr<osg::Camera> camera = mViewer->getCamera();

        camera->setClearMask(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
        camera->setClearColor(osg::Vec4f(0.2f, 0.2f, 0.4f, 1.0f));

        // Add the Camera to the Viewer
        mViewer->setCamera(camera.get());

        // Add the Camera Manipulator to the Viewer
        // mViewer->setCameraManipulator(keyswitchManipulator.get());
        osg::ref_ptr<osgGA::TrackballManipulator> trackball = new osgGA::TrackballManipulator();
        mViewer->setCameraManipulator(trackball.get());

        mViewer->setSceneData(loadedModel);

        // // Realize the Viewer's graphics context, which already done in the default pWidget
        // mViewer->realize(); 

        return app.exec();
    }
}

+// add these marcos to fix:  
+// 1>f:\prjs\orb_slam3\thirdparty\g2o\g2o\core\sparse_block_matrix.hpp(277): fatal error C1001: 编译器中发生内部错误。
+
+#define _AXPY(MatrixType,A,x,xoff,y,yoff)y.segment<MatrixType::RowsAtCompileTime>(yoff) += (A) * (x).segment<MatrixType::ColsAtCompileTime>(xoff)
+#define _ATXPY(MatrixType,A,x,xoff,y,yoff)y.segment<MatrixType::ColsAtCompileTime>(yoff) += (A).transpose() * (x).segment<MatrixType::RowsAtCompileTime>(xoff)
+
+
 namespace g2o {
   using namespace Eigen;
 
@@ -249,7 +256,8 @@ namespace g2o {
         const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
         int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;
         // destVec += *a * srcVec (according to the sub-vector parts)
-        internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
+        // internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
+        _AXPY(MatrixType, *a, srcVec, srcOffset, destVec, destOffset);
       }
     }
   }
@@ -274,9 +282,12 @@ namespace g2o {
         if (destOffset > srcOffset) // only upper triangle
           break;
         // destVec += *a * srcVec (according to the sub-vector parts)
-        internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
-        if (destOffset < srcOffset)
-          internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset);
+        // internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
+        _AXPY(MatrixType, *a, srcVec, srcOffset, destVec, destOffset);
+        if (destOffset < srcOffset) {
+          // internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset);
+          _ATXPY(MatrixType, *a, srcVec, destOffset, destVec, srcOffset);
+        }
       }
     }
   }
@@ -305,7 +316,8 @@ namespace g2o {
         const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
         int srcOffset = rowBaseOfBlock(it->first);
         // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
-        internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+        // internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+	    _ATXPY(MatrixType, *a, srcVec, srcOffset, destVec, destOffset);
       }
     }
     
diff --git a/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h b/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
index 36ddfe6..91832cd 100644
--- a/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
+++ b/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
@@ -24,6 +24,9 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#define _AXPY(MatrixType,A,x,xoff,y,yoff)y.segment<MatrixType::RowsAtCompileTime>(yoff) += (A) * (x).segment<MatrixType::ColsAtCompileTime>(xoff)
+#define _ATXPY(MatrixType,A,x,xoff,y,yoff)y.segment<MatrixType::ColsAtCompileTime>(yoff) += (A).transpose() * (x).segment<MatrixType::RowsAtCompileTime>(xoff)
+
 #ifndef G2O_SPARSE_BLOCK_MATRIX_CCS_H
 #define G2O_SPARSE_BLOCK_MATRIX_CCS_H
 
@@ -122,7 +125,8 @@ namespace g2o {
             const SparseMatrixBlock* a = it->block;
             int srcOffset = rowBaseOfBlock(it->row);
             // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
-            internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+            // internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+            _ATXPY(MatrixType, *a, srcVec, srcOffset, destVec, destOffset);
           }
         }
       }
diff --git a/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h b/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h
index 7b13b9f..8605a5f 100644
--- a/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h
+++ b/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h
@@ -24,6 +24,9 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
+#define _AXPY(MatrixType,A,x,xoff,y,yoff)y.segment<MatrixType::RowsAtCompileTime>(yoff) += (A) * (x).segment<MatrixType::ColsAtCompileTime>(xoff)
+#define _ATXPY(MatrixType,A,x,xoff,y,yoff)y.segment<MatrixType::ColsAtCompileTime>(yoff) += (A).transpose() * (x).segment<MatrixType::RowsAtCompileTime>(xoff)
+
 #ifndef G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
 #define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
 
@@ -94,7 +97,8 @@ namespace g2o {
           int srcOffset = destOffset;
           const SparseMatrixBlock& A = _diagonal[i];
           // destVec += *A.transpose() * srcVec (according to the sub-vector parts)
-          internal::axpy(A, srcVec, srcOffset, destVec, destOffset);
+          // internal::axpy(A, srcVec, srcOffset, destVec, destOffset);
+          _AXPY(MatrixType, A, srcVec, srcOffset, destVec, destOffset);
         }
       }

```cpp
#include <windows.h>

void usleep(__int64 usec) 
{ 
    HANDLE timer; 
    LARGE_INTEGER ft; 

    ft.QuadPart = -(10*usec); // Convert to 100 nanosecond interval, negative value indicates relative time

    timer = CreateWaitableTimer(NULL, TRUE, NULL); 
    SetWaitableTimer(timer, &ft, 0, NULL, NULL, 0); 
    WaitForSingleObject(timer, INFINITE); 
    CloseHandle(timer); 
}

if (ttrack < T) {
    long usec = static_cast<long>((T - ttrack) * 1e6);
    std::this_thread::sleep_for(std::chrono::microseconds(usec));
}

#include <stdlib.h>
#include <stdio.h>
#include "Paxos/Acceptor.h"
#include "Paxos/Proposer.h"
#include "lib/Thread.h"
#include "lib/Lock.h"
#include "lib/mapi.h"
#include "lib/atom.h"
#include "lib/Logger.h"

paxos::Proposer p[5];
paxos::Acceptor a[11];
mdk::Mutex l[11];
int finishedCount = 0;
int finalValue = -1;
bool isFinished = false;
mdk::uint64 g_start;
mdk::Logger g_log;

void* Proposer(void *id)
{
	mdk::Logger log;
	char logName[256];
	sprintf( logName, "Proposer%d", (long)id );
	log.SetLogName(logName);
	log.SetMaxLogSize(10);
	log.SetMaxExistDay(30);
	log.SetPrintLog(false);

	paxos::Proposer &proposer = p[(long)id];
	paxos::PROPOSAL value = proposer.GetProposal();
	paxos::PROPOSAL lastValue;


	int acceptorId[11];
	int count = 0;

	mdk::uint64 start = mdk::MillTime();
	while ( true )
	{
		value = proposer.GetProposal();//拿到提议
		log.Info("Info", "Proposer%d号开始(Propose阶段):提议=[编号:%d，提议:%d]\n", 
			(long)id, value.serialNum, value.value);
		count = 0;
		int i = 0;
		for (i = 0; i < 11; i++ )
		{
		/*
			* 发送消息到第i个acceptor
			* 经过一定时间达到acceptor，sleep(随机时间)模拟
			* acceptor处理消息，mAcceptors[i].Propose()
			* 回应proposer
			* 经过一定时间proposer收到回应，sleep(随机时间)模拟
			* proposer处理回应mProposer.proposed(ok, lastValue)
		*/
			mdk::m_sleep(rand()%500);//经过随机时间，消息到达了mAcceptors[i]
			//处理消息
			l[i].Lock();
			bool ok = a[i].Propose(value.serialNum, lastValue);
			l[i].Unlock();
			mdk::m_sleep(rand()%500);//经过随机时间,消息到达Proposer
			//处理Propose回应
			if ( !proposer.Proposed(ok, lastValue) ) //重新开始Propose阶段
			{
				mdk::m_sleep(1000);//为了降低活锁，多等一会让别的proposer有机会完成自己的2阶段批准
				break;
			}
			paxos::PROPOSAL curValue = proposer.GetProposal();//拿到提议
			if ( curValue.value != value.value )//acceptor本次回应可能推荐了一个提议
			{
				log.Info("Info", "Proposer%d号修改了提议:提议=[编号:%d，提议:%d]\n", 
					(long)id, curValue.serialNum, curValue.value);
				break;
			}
			acceptorId[count++] = i;//记录愿意投票的acceptor
			if ( proposer.StartAccept() ) break;
		}
		//检查有没有达到Accept开始条件，如果没有表示要重新开始Propose阶段
		if ( !proposer.StartAccept() ) continue;

		//开始Accept阶段
		//发送Accept消息到所有愿意投票的acceptor
		value = proposer.GetProposal();
		log.Info("Info", "Proposer%d号开始(Accept阶段):提议=[编号:%d，提议:%d]\n", 
			(long)id, value.serialNum, value.value);
		for (i = 0; i < count; i++ )
		{
			//发送accept消息到acceptor
			//减少accept阶段等待时间，加快收敛
			mdk::m_sleep(rand()%200);//经过随机时间,accept消息到达acceptor
			//处理accept消息
			l[acceptorId[i]].Lock();
			bool ok = a[acceptorId[i]].Accept(value);
			l[acceptorId[i]].Unlock();
			mdk::m_sleep(rand()%200);//经过随机时间,accept回应到达proposer
			//处理accept回应
			if ( !proposer.Accepted(ok) ) //重新开始Propose阶段
			{
				mdk::m_sleep(1000);//为了降低活锁，多等一会让别的proposer有机会完成自己的2阶段批准
				break;
			}
			if ( proposer.IsAgree() )//成功批准了提议
			{
				start = mdk::MillTime() - start;
				log.Info("Info", "Proposer%d号的提议被批准,用时%lluMS:最终提议 = [编号:%d，提议:%d]\n", (long)id, start, value.serialNum, value.value);
				g_log.Info("Info", "Proposer%d号的提议被批准,用时%lluMS:最终提议 = [编号:%d，提议:%d]\n", (long)id, start, value.serialNum, value.value);
				if(finalValue == -1) finalValue = value.value;
				else if(finalValue != value.value) finalValue = 0;
				if ( 4 == mdk::AtomAdd(&finishedCount, 1) )
				{
					isFinished = true;
					g_start = mdk::MillTime() - g_start;
					if(finalValue > 0){
						g_log.Info("Info", "Paxos完成，用时%lluMS，最终通过提议值为：%d\n", g_start, finalValue);
					}
					else{
						g_log.Info("Info", "Paxos完成，用时%lluMS，最终结果不一致！\n", g_start);
					}
				}
				return NULL;
			}
		}
	}
	return NULL;
}

//Paxos过程模拟演示程序
int main(int argc, char* argv[])
{
	int i = 0;
	g_log.SetLogName("Paxos");
	g_log.SetMaxLogSize(10);
	g_log.SetMaxExistDay(30);
	g_log.SetPrintLog(true);
	g_log.Info("Info", "5个Proposer, 11个Acceptor准备进行Paxos\n"
		"每个Proposer独立线程，Acceptor不需要线程\n"
		"Proposer编号从0-10,编号为i的Proposer初始提议编号和提议值是（i+1, i+1）\n"
		"Proposer每次重新提议会将提议编号增加5\n"
		"Proposer被批准后结束线程,其它线程继续投票最终，全部批准相同的值，达成一致。\n");
	g_start = mdk::MillTime();
	g_log.Info("Info", "Paxos开始\n" );
	paxos::PROPOSAL value;

	for ( i = 0; i < 5; i++ ) 
	{
		p[i].SetPlayerCount(5, 11);
		value.serialNum = value.value = i + 1;
		p[i].StartPropose(value);
	}

	mdk::Thread t[5];
	for ( i = 0; i < 5; i++ ) t[i].Run(Proposer, (void*)i);
	//for ( i = 0; i < 5; i++ ) t[i].WaitStop();
	while(true){
		if(isFinished) break;
		mdk::m_sleep(500);
	}
	return 0;
}