diff --git a/.github/actions/pregen/action.yml b/.github/actions/pregen/action.yml index 2c61be413a..3c251432bf 100644 --- a/.github/actions/pregen/action.yml +++ b/.github/actions/pregen/action.yml @@ -11,10 +11,10 @@ runs: - name: Install jinja and protobuf run: python -m pip install jinja2 protobuf grpcio-tools shell: bash - - name: Install protobuf dependencies + - name: Install protobuf and perl dependencies run: | sudo apt-get update - sudo apt-get install -y protobuf-compiler + sudo apt-get install -y protobuf-compiler liblist-moreutils-perl wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe shell: bash @@ -45,6 +45,10 @@ runs: ./wpilibj/generate_pwm_motor_controllers.py shell: bash + - name: Regenerate mrcal minimath + run: ./wpical/generate_mrcal.py + shell: bash + - name: Regenerate wpimath run: | ./wpimath/generate_nanopb.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f689f3af0a..9448fc87bf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ cmake_dependent_option( ) option(WITH_CSCORE "Build cscore (needs OpenCV)" ON) option(WITH_NTCORE "Build ntcore" ON) +option(WITH_WPICAL "Build wpical" OFF) option(WITH_WPIMATH "Build wpimath" ON) cmake_dependent_option( WITH_WPIUNITS @@ -142,6 +143,11 @@ if(WITH_DOCS) include(AddDoxygenDocs) add_doxygen_docs() endif() + +if(WITH_WPICAL) + find_package(Ceres CONFIG REQUIRED) +endif() + find_package(LIBSSH CONFIG 0.7.1) set(CMAKE_FIND_PACKAGE_PREFER_CONFIG ON) @@ -314,6 +320,9 @@ if(WITH_GUI) add_subdirectory(glass) add_subdirectory(outlineviewer) add_subdirectory(sysid) + if(WITH_WPICAL) + add_subdirectory(wpical) + endif() if(LIBSSH_FOUND) add_subdirectory(roborioteamnumbersetter) add_subdirectory(datalogtool) diff --git a/README.md b/README.md index af2fcf37ff..3576047006 100644 --- a/README.md +++ b/README.md @@ -59,6 +59,8 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen On macOS ARM, run `softwareupdate --install-rosetta`. This is necessary to be able to use the macOS x86 roboRIO toolchain on ARM. +On linux, run `sudo apt install gfortran`. This is necessary to be able to build WPIcal on linux platforms. + ## Setup Clone the WPILib repository and follow the instructions above for installing any required tooling. The build process uses versioning information from git. Downloading the source is not sufficient to run the build. diff --git a/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java b/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java index 14e40ba140..0b5bd3239d 100644 --- a/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java +++ b/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java @@ -44,7 +44,7 @@ public class CvSink extends ImageSink { * Create a sink for accepting OpenCV images. grabFrame() must be called on the created sink to * get each new image. * - * @param name Source name (arbitrary unique identifier) + * @param name Sink name (arbitrary unique identifier) * @param pixelFormat Source pixel format */ public CvSink(String name, PixelFormat pixelFormat) { diff --git a/cscore/src/main/native/cpp/RawSinkImpl.cpp b/cscore/src/main/native/cpp/RawSinkImpl.cpp index 3e38cb81c3..355b0913e4 100644 --- a/cscore/src/main/native/cpp/RawSinkImpl.cpp +++ b/cscore/src/main/native/cpp/RawSinkImpl.cpp @@ -63,6 +63,11 @@ uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image) { } uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image, double timeout) { + return GrabFrame(image, timeout, 0); +} + +uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image, double timeout, + uint64_t lastFrameTime) { SetEnabled(true); auto source = GetSource(); @@ -72,7 +77,7 @@ uint64_t RawSinkImpl::GrabFrame(WPI_RawFrame& image, double timeout) { return 0; } - auto frame = source->GetNextFrame(timeout); // blocks + auto frame = source->GetNextFrame(timeout, lastFrameTime); // blocks if (!frame) { // Bad frame; sleep for 20 ms so we don't consume all processor time. std::this_thread::sleep_for(std::chrono::milliseconds(20)); @@ -183,6 +188,18 @@ uint64_t GrabSinkFrameTimeout(CS_Sink sink, WPI_RawFrame& image, double timeout, return static_cast(*data->sink).GrabFrame(image, timeout); } +uint64_t GrabSinkFrameTimeoutLastTime(CS_Sink sink, WPI_RawFrame& image, + double timeout, uint64_t lastFrameTime, + CS_Status* status) { + auto data = Instance::GetInstance().GetSink(sink); + if (!data || (data->kind & SinkMask) == 0) { + *status = CS_INVALID_HANDLE; + return 0; + } + return static_cast(*data->sink) + .GrabFrame(image, timeout, lastFrameTime); +} + } // namespace cs extern "C" { @@ -209,4 +226,13 @@ uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct WPI_RawFrame* image, return cs::GrabSinkFrameTimeout(sink, *image, timeout, status); } +uint64_t CS_GrabRawSinkFrameTimeoutWithFrameTime(CS_Sink sink, + struct WPI_RawFrame* image, + double timeout, + uint64_t lastFrameTime, + CS_Status* status) { + return cs::GrabSinkFrameTimeoutLastTime(sink, *image, timeout, lastFrameTime, + status); +} + } // extern "C" diff --git a/cscore/src/main/native/cpp/RawSinkImpl.h b/cscore/src/main/native/cpp/RawSinkImpl.h index ac719764ec..423f3b4b22 100644 --- a/cscore/src/main/native/cpp/RawSinkImpl.h +++ b/cscore/src/main/native/cpp/RawSinkImpl.h @@ -34,10 +34,15 @@ class RawSinkImpl : public SinkImpl { uint64_t GrabFrame(WPI_RawFrame& frame); uint64_t GrabFrame(WPI_RawFrame& frame, double timeout); + // Wait for a frame with a time other than lastFrameTime + uint64_t GrabFrame(WPI_RawFrame& frame, double timeout, + uint64_t lastFrameTime); private: void ThreadMain(); + // Copies the image from incomingFrame into rawFrame, converting where + // necessary to the resolution of rawFrame uint64_t GrabFrameImpl(WPI_RawFrame& rawFrame, Frame& incomingFrame); std::atomic_bool m_active; // set to false to terminate threads diff --git a/cscore/src/main/native/cpp/SourceImpl.cpp b/cscore/src/main/native/cpp/SourceImpl.cpp index f2cc0d8ccc..067ba15014 100644 --- a/cscore/src/main/native/cpp/SourceImpl.cpp +++ b/cscore/src/main/native/cpp/SourceImpl.cpp @@ -84,12 +84,17 @@ Frame SourceImpl::GetNextFrame() { return m_frame; } -Frame SourceImpl::GetNextFrame(double timeout) { +Frame SourceImpl::GetNextFrame(double timeout, Frame::Time lastFrameTime) { std::unique_lock lock{m_frameMutex}; - auto oldTime = m_frame.GetTime(); + + if (lastFrameTime == 0) { + lastFrameTime = m_frame.GetTime(); + } + + // Wait unitl m_frame has a timestamp other than lastFrameTime if (!m_frameCv.wait_for( lock, std::chrono::milliseconds(static_cast(timeout * 1000)), - [=, this] { return m_frame.GetTime() != oldTime; })) { + [=, this] { return m_frame.GetTime() != lastFrameTime; })) { m_frame = Frame{*this, "timed out getting frame", wpi::Now()}; } return m_frame; diff --git a/cscore/src/main/native/cpp/SourceImpl.h b/cscore/src/main/native/cpp/SourceImpl.h index 840fb5d7ed..f023e81f2d 100644 --- a/cscore/src/main/native/cpp/SourceImpl.h +++ b/cscore/src/main/native/cpp/SourceImpl.h @@ -98,7 +98,8 @@ class SourceImpl : public PropertyContainer { // Blocking function that waits for the next frame and returns it (with // timeout in seconds). If timeout expires, returns empty frame. - Frame GetNextFrame(double timeout); + // If lastFrameTime==0, uses m_frame.GetTime() for lastFrameTime + Frame GetNextFrame(double timeout, Frame::Time lastFrameTime = 0); // Force a wakeup of all GetNextFrame() callers by sending an empty frame. void Wakeup(); diff --git a/cscore/src/main/native/include/cscore_cv.h b/cscore/src/main/native/include/cscore_cv.h index 237c3946af..d703582c06 100644 --- a/cscore/src/main/native/include/cscore_cv.h +++ b/cscore/src/main/native/include/cscore_cv.h @@ -151,6 +151,27 @@ class CvSink : public ImageSink { [[nodiscard]] uint64_t GrabFrameNoTimeoutDirect(cv::Mat& image); + /** + * Wait for the next frame and get the image. + * Times out (returning 0) after timeout seconds. + * The provided image will have the pixelFormat this class was constructed + * with. The data is backed by data in the CvSink. It will be invalidated by + * any grabFrame*() call on the sink. + * + *

If lastFrameTime is provided and non-zero, the sink will fill image with + * the first frame from the source that is not equal to lastFrameTime. If + * lastFrameTime is zero, the time of the current frame owned by the CvSource + * is used, and this function will block until the connected CvSource provides + * a new frame. + * + * @return Frame time, or 0 on error (call GetError() to obtain the error + * message); the frame time is in the same time base as wpi::Now(), + * and is in 1 us increments. + */ + [[nodiscard]] + uint64_t GrabFrameDirectLastTime(cv::Mat& image, uint64_t lastFrameTime, + double timeout = 0.225); + private: constexpr int GetCvFormat(WPI_PixelFormat pixelFormat); @@ -365,6 +386,25 @@ inline uint64_t CvSink::GrabFrameNoTimeoutDirect(cv::Mat& image) { return timestamp; } +inline uint64_t CvSink::GrabFrameDirectLastTime(cv::Mat& image, + uint64_t lastFrameTime, + double timeout) { + rawFrame.height = 0; + rawFrame.width = 0; + rawFrame.stride = 0; + rawFrame.pixelFormat = pixelFormat; + auto timestamp = GrabSinkFrameTimeoutLastTime(m_handle, rawFrame, timeout, + lastFrameTime, &m_status); + if (m_status != CS_OK) { + return 0; + } + image = + cv::Mat{rawFrame.height, rawFrame.width, + GetCvFormat(static_cast(rawFrame.pixelFormat)), + rawFrame.data, static_cast(rawFrame.stride)}; + return timestamp; +} + } // namespace cs #endif // CSCORE_CSCORE_CV_H_ diff --git a/cscore/src/main/native/include/cscore_raw.h b/cscore/src/main/native/include/cscore_raw.h index 6333bca655..e7a47fc9ee 100644 --- a/cscore/src/main/native/include/cscore_raw.h +++ b/cscore/src/main/native/include/cscore_raw.h @@ -27,6 +27,11 @@ uint64_t CS_GrabRawSinkFrame(CS_Sink sink, struct WPI_RawFrame* rawImage, CS_Status* status); uint64_t CS_GrabRawSinkFrameTimeout(CS_Sink sink, struct WPI_RawFrame* rawImage, double timeout, CS_Status* status); +uint64_t CS_GrabRawSinkFrameTimeoutWithFrameTime(CS_Sink sink, + struct WPI_RawFrame* rawImage, + double timeout, + uint64_t lastFrameTime, + CS_Status* status); CS_Sink CS_CreateRawSink(const struct WPI_String* name, CS_Bool isCv, CS_Status* status); @@ -67,6 +72,9 @@ void PutSourceFrame(CS_Source source, const WPI_RawFrame& image, uint64_t GrabSinkFrame(CS_Sink sink, WPI_RawFrame& image, CS_Status* status); uint64_t GrabSinkFrameTimeout(CS_Sink sink, WPI_RawFrame& image, double timeout, CS_Status* status); +uint64_t GrabSinkFrameTimeoutLastTime(CS_Sink sink, WPI_RawFrame& image, + double timeout, uint64_t lastFrameTime, + CS_Status* status); /** * A source for user code to provide video frames as raw bytes. @@ -163,6 +171,24 @@ class RawSink : public ImageSink { */ [[nodiscard]] uint64_t GrabFrameNoTimeout(wpi::RawFrame& image) const; + + /** + * Wait for the next frame and get the image. May block forever. + * The provided image will have three 8-bit channels stored in BGR order. + * + *

If lastFrameTime is provided and non-zero, the sink will fill image with + * the first frame from the source that is not equal to lastFrameTime. If + * lastFrameTime is zero, the time of the current frame owned by the CvSource + * is used, and this function will block until the connected CvSource provides + * a new frame. + * + * @return Frame time, or 0 on error (call GetError() to obtain the error + * message); the frame time is in the same time base as wpi::Now(), + * and is in 1 us increments. + */ + [[nodiscard]] + uint64_t GrabFrameLastTime(wpi::RawFrame& image, uint64_t lastFrameTime, + double timeout = 0.225) const; }; inline RawSource::RawSource(std::string_view name, const VideoMode& mode) { @@ -199,6 +225,14 @@ inline uint64_t RawSink::GrabFrameNoTimeout(wpi::RawFrame& image) const { m_status = 0; return GrabSinkFrame(m_handle, image, &m_status); } + +inline uint64_t RawSink::GrabFrameLastTime(wpi::RawFrame& image, + uint64_t lastFrameTime, + double timeout) const { + m_status = 0; + return GrabSinkFrameTimeoutLastTime(m_handle, image, timeout, lastFrameTime, + &m_status); +} /** @} */ } // namespace cs diff --git a/ntcore/src/main/native/include/networktables/GenericEntry.h b/ntcore/src/main/native/include/networktables/GenericEntry.h index e71a25afad..449daab2f5 100644 --- a/ntcore/src/main/native/include/networktables/GenericEntry.h +++ b/ntcore/src/main/native/include/networktables/GenericEntry.h @@ -387,7 +387,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultBoolean(bool defaultValue) { return nt::SetDefaultBoolean(m_pubHandle, defaultValue); @@ -397,7 +397,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultInteger(int64_t defaultValue) { return nt::SetDefaultInteger(m_pubHandle, defaultValue); @@ -407,7 +407,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultFloat(float defaultValue) { return nt::SetDefaultFloat(m_pubHandle, defaultValue); @@ -417,7 +417,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultDouble(double defaultValue) { return nt::SetDefaultDouble(m_pubHandle, defaultValue); @@ -427,7 +427,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultString(std::string_view defaultValue) { return nt::SetDefaultString(m_pubHandle, defaultValue); @@ -437,7 +437,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultRaw(std::span defaultValue) { return nt::SetDefaultRaw(m_pubHandle, defaultValue); @@ -447,7 +447,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultBooleanArray(std::span defaultValue) { return nt::SetDefaultBooleanArray(m_pubHandle, defaultValue); @@ -457,7 +457,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultIntegerArray(std::span defaultValue) { return nt::SetDefaultIntegerArray(m_pubHandle, defaultValue); @@ -467,7 +467,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultFloatArray(std::span defaultValue) { return nt::SetDefaultFloatArray(m_pubHandle, defaultValue); @@ -477,7 +477,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultDoubleArray(std::span defaultValue) { return nt::SetDefaultDoubleArray(m_pubHandle, defaultValue); @@ -487,7 +487,7 @@ class GenericPublisher : public Publisher { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultStringArray(std::span defaultValue) { return nt::SetDefaultStringArray(m_pubHandle, defaultValue); diff --git a/ntcore/src/main/native/include/networktables/NetworkTable.h b/ntcore/src/main/native/include/networktables/NetworkTable.h index 1008859171..b887337267 100644 --- a/ntcore/src/main/native/include/networktables/NetworkTable.h +++ b/ntcore/src/main/native/include/networktables/NetworkTable.h @@ -361,11 +361,11 @@ class NetworkTable final { bool PutNumber(std::string_view key, double value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @returns True if the table key did not already exist, otherwise False */ bool SetDefaultNumber(std::string_view key, double defaultValue); @@ -389,11 +389,11 @@ class NetworkTable final { bool PutString(std::string_view key, std::string_view value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @returns True if the table key did not already exist, otherwise False */ bool SetDefaultString(std::string_view key, std::string_view defaultValue); @@ -419,11 +419,11 @@ class NetworkTable final { bool PutBoolean(std::string_view key, bool value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @returns True if the table key did not already exist, otherwise False */ bool SetDefaultBoolean(std::string_view key, bool defaultValue); @@ -452,11 +452,11 @@ class NetworkTable final { bool PutBooleanArray(std::string_view key, std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @return False if the table key exists with a different type + * @return True if the table key did not already exist, otherwise False */ bool SetDefaultBooleanArray(std::string_view key, std::span defaultValue); @@ -490,11 +490,11 @@ class NetworkTable final { bool PutNumberArray(std::string_view key, std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @returns True if the table key did not already exist, otherwise False */ bool SetDefaultNumberArray(std::string_view key, std::span defaultValue); @@ -524,11 +524,11 @@ class NetworkTable final { bool PutStringArray(std::string_view key, std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @returns True if the table key did not already exist, otherwise False */ bool SetDefaultStringArray(std::string_view key, std::span defaultValue); @@ -558,11 +558,11 @@ class NetworkTable final { bool PutRaw(std::string_view key, std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @return False if the table key exists with a different type + * @return True if the table key did not already exist, otherwise False */ bool SetDefaultRaw(std::string_view key, std::span defaultValue); @@ -592,11 +592,11 @@ class NetworkTable final { bool PutValue(std::string_view key, const Value& value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set Default Entry Value. * * @param key the key * @param defaultValue the default value to set if key doesn't exist. - * @return False if the table key exists with a different type + * @return True if the table key did not already exist, otherwise False */ bool SetDefaultValue(std::string_view key, const Value& defaultValue); diff --git a/ntcore/src/main/native/include/networktables/NetworkTableEntry.h b/ntcore/src/main/native/include/networktables/NetworkTableEntry.h index 6b4cd11a85..789d4361bc 100644 --- a/ntcore/src/main/native/include/networktables/NetworkTableEntry.h +++ b/ntcore/src/main/native/include/networktables/NetworkTableEntry.h @@ -260,7 +260,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultValue(const Value& defaultValue) { return SetDefaultEntryValue(m_handle, defaultValue); @@ -270,7 +270,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultBoolean(bool defaultValue) { return nt::SetDefaultBoolean(m_handle, defaultValue); @@ -280,7 +280,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultInteger(int64_t defaultValue) { return nt::SetDefaultInteger(m_handle, defaultValue); @@ -290,7 +290,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultFloat(float defaultValue) { return nt::SetDefaultFloat(m_handle, defaultValue); @@ -300,7 +300,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultDouble(double defaultValue) { return nt::SetDefaultDouble(m_handle, defaultValue); @@ -310,7 +310,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultString(std::string_view defaultValue) { return nt::SetDefaultString(m_handle, defaultValue); @@ -320,7 +320,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultRaw(std::span defaultValue) { return nt::SetDefaultRaw(m_handle, defaultValue); @@ -330,7 +330,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultBooleanArray(std::span defaultValue) { return nt::SetDefaultBooleanArray(m_handle, defaultValue); @@ -340,7 +340,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultIntegerArray(std::span defaultValue) { return nt::SetDefaultIntegerArray(m_handle, defaultValue); @@ -350,7 +350,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultFloatArray(std::span defaultValue) { return nt::SetDefaultFloatArray(m_handle, defaultValue); @@ -360,7 +360,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultDoubleArray(std::span defaultValue) { return nt::SetDefaultDoubleArray(m_handle, defaultValue); @@ -370,7 +370,7 @@ class NetworkTableEntry final { * Sets the entry's value if it does not exist. * * @param defaultValue the default value to set - * @return False if the entry exists with a different type + * @return True if the entry did not already have a value, otherwise False */ bool SetDefaultStringArray(std::span defaultValue) { return nt::SetDefaultStringArray(m_handle, defaultValue); diff --git a/ntcore/src/main/native/include/ntcore_c.h b/ntcore/src/main/native/include/ntcore_c.h index e918aaafa0..4489b4f382 100644 --- a/ntcore/src/main/native/include/ntcore_c.h +++ b/ntcore/src/main/native/include/ntcore_c.h @@ -476,8 +476,8 @@ void NT_GetEntryValueType(NT_Entry entry, unsigned int types, /** * Set Default Entry Value. * - * Returns copy of current entry value if it exists. - * Otherwise, sets passed in value, and returns set value. + * Returns 0 if name exists. + * Otherwise, sets passed in value, and returns 1. * Note that one of the type options is "unassigned". * * @param entry entry handle diff --git a/ntcore/src/main/native/include/ntcore_cpp.h b/ntcore/src/main/native/include/ntcore_cpp.h index 226669927c..b64106ecff 100644 --- a/ntcore/src/main/native/include/ntcore_cpp.h +++ b/ntcore/src/main/native/include/ntcore_cpp.h @@ -488,8 +488,8 @@ Value GetEntryValue(NT_Handle subentry); /** * Set Default Entry Value * - * Returns copy of current entry value if it exists. - * Otherwise, sets passed in value, and returns set value. + * Returns False if name exists. + * Otherwise, sets passed in value, and returns True. * Note that one of the type options is "unassigned". * * @param entry entry handle diff --git a/settings.gradle b/settings.gradle index 298d00149f..b81e6280b8 100644 --- a/settings.gradle +++ b/settings.gradle @@ -62,6 +62,7 @@ include 'epilogue-processor' include 'epilogue-runtime' include 'thirdparty:googletest' include 'thirdparty:imgui_suite' +include 'wpical' buildCache { def cred = { diff --git a/shared/ceres.gradle b/shared/ceres.gradle new file mode 100644 index 0000000000..550fd6c9e7 --- /dev/null +++ b/shared/ceres.gradle @@ -0,0 +1,13 @@ +nativeUtils { + nativeDependencyContainer { + ceres(getNativeDependencyTypeClass('WPIStaticMavenDependency')) { + groupId = "edu.wpi.first.thirdparty.frc2024.ceres" + artifactId = "ceres-cpp" + headerClassifier = "headers" + sourceClassifier = "sources" + ext = "zip" + version = '2.2-3' + targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms) + } + } +} diff --git a/thirdparty/imgui_suite/imgui/cpp/imgui_demo.cpp b/thirdparty/imgui_suite/imgui/cpp/imgui_demo.cpp new file mode 100644 index 0000000000..51be2777ef --- /dev/null +++ b/thirdparty/imgui_suite/imgui/cpp/imgui_demo.cpp @@ -0,0 +1,8570 @@ +// dear imgui, v1.90 WIP +// (demo code) + +// Help: +// - Read FAQ at http://dearimgui.com/faq +// - Call and read ImGui::ShowDemoWindow() in imgui_demo.cpp. All applications in examples/ are doing that. +// - Need help integrating Dear ImGui in your codebase? +// - Read Getting Started https://github.com/ocornut/imgui/wiki/Getting-Started +// - Read 'Programmer guide' in imgui.cpp for notes on how to setup Dear ImGui in your codebase. +// Read imgui.cpp for more details, documentation and comments. +// Get the latest version at https://github.com/ocornut/imgui + +//--------------------------------------------------- +// PLEASE DO NOT REMOVE THIS FILE FROM YOUR PROJECT! +//--------------------------------------------------- +// Message to the person tempted to delete this file when integrating Dear ImGui into their codebase: +// Think again! It is the most useful reference code that you and other coders will want to refer to and call. +// Have the ImGui::ShowDemoWindow() function wired in an always-available debug menu of your game/app! +// Also include Metrics! ItemPicker! DebugLog! and other debug features. +// Removing this file from your project is hindering access to documentation for everyone in your team, +// likely leading you to poorer usage of the library. +// Everything in this file will be stripped out by the linker if you don't call ImGui::ShowDemoWindow(). +// If you want to link core Dear ImGui in your shipped builds but want a thorough guarantee that the demo will not be +// linked, you can setup your imconfig.h with #define IMGUI_DISABLE_DEMO_WINDOWS and those functions will be empty. +// In another situation, whenever you have Dear ImGui available you probably want this to be available for reference. +// Thank you, +// -Your beloved friend, imgui_demo.cpp (which you won't delete) + +//-------------------------------------------- +// ABOUT THE MEANING OF THE 'static' KEYWORD: +//-------------------------------------------- +// In this demo code, we frequently use 'static' variables inside functions. +// A static variable persists across calls. It is essentially a global variable but declared inside the scope of the function. +// Think of "static int n = 0;" as "global int n = 0;" ! +// We do this IN THE DEMO because we want: +// - to gather code and data in the same place. +// - to make the demo source code faster to read, faster to change, smaller in size. +// - it is also a convenient way of storing simple UI related information as long as your function +// doesn't need to be reentrant or used in multiple threads. +// This might be a pattern you will want to use in your code, but most of the data you would be working +// with in a complex codebase is likely going to be stored outside your functions. + +//----------------------------------------- +// ABOUT THE CODING STYLE OF OUR DEMO CODE +//----------------------------------------- +// The Demo code in this file is designed to be easy to copy-and-paste into your application! +// Because of this: +// - We never omit the ImGui:: prefix when calling functions, even though most code here is in the same namespace. +// - We try to declare static variables in the local scope, as close as possible to the code using them. +// - We never use any of the helpers/facilities used internally by Dear ImGui, unless available in the public API. +// - We never use maths operators on ImVec2/ImVec4. For our other sources files we use them, and they are provided +// by imgui.h using the IMGUI_DEFINE_MATH_OPERATORS define. For your own sources file they are optional +// and require you either enable those, either provide your own via IM_VEC2_CLASS_EXTRA in imconfig.h. +// Because we can't assume anything about your support of maths operators, we cannot use them in imgui_demo.cpp. + +// Navigating this file: +// - In Visual Studio IDE: CTRL+comma ("Edit.GoToAll") can follow symbols in comments, whereas CTRL+F12 ("Edit.GoToImplementation") cannot. +// - With Visual Assist installed: ALT+G ("VAssistX.GoToImplementation") can also follow symbols in comments. + +/* + +Index of this file: + +// [SECTION] Forward Declarations +// [SECTION] Helpers +// [SECTION] Demo Window / ShowDemoWindow() +// - ShowDemoWindow() +// - sub section: ShowDemoWindowWidgets() +// - sub section: ShowDemoWindowLayout() +// - sub section: ShowDemoWindowPopups() +// - sub section: ShowDemoWindowTables() +// - sub section: ShowDemoWindowInputs() +// [SECTION] About Window / ShowAboutWindow() +// [SECTION] Style Editor / ShowStyleEditor() +// [SECTION] User Guide / ShowUserGuide() +// [SECTION] Example App: Main Menu Bar / ShowExampleAppMainMenuBar() +// [SECTION] Example App: Debug Console / ShowExampleAppConsole() +// [SECTION] Example App: Debug Log / ShowExampleAppLog() +// [SECTION] Example App: Simple Layout / ShowExampleAppLayout() +// [SECTION] Example App: Property Editor / ShowExampleAppPropertyEditor() +// [SECTION] Example App: Long Text / ShowExampleAppLongText() +// [SECTION] Example App: Auto Resize / ShowExampleAppAutoResize() +// [SECTION] Example App: Constrained Resize / ShowExampleAppConstrainedResize() +// [SECTION] Example App: Simple overlay / ShowExampleAppSimpleOverlay() +// [SECTION] Example App: Fullscreen window / ShowExampleAppFullscreen() +// [SECTION] Example App: Manipulating window titles / ShowExampleAppWindowTitles() +// [SECTION] Example App: Custom Rendering using ImDrawList API / ShowExampleAppCustomRendering() +// [SECTION] Example App: Docking, DockSpace / ShowExampleAppDockSpace() +// [SECTION] Example App: Documents Handling / ShowExampleAppDocuments() + +*/ + +#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) +#define _CRT_SECURE_NO_WARNINGS +#endif + +#include "imgui.h" +#ifndef IMGUI_DISABLE + +// System includes +#include // toupper +#include // INT_MIN, INT_MAX +#include // sqrtf, powf, cosf, sinf, floorf, ceilf +#include // vsnprintf, sscanf, printf +#include // NULL, malloc, free, atoi +#include // intptr_t +#if !defined(_MSC_VER) || _MSC_VER >= 1800 +#include // PRId64/PRIu64, not avail in some MinGW headers. +#endif + +// Visual Studio warnings +#ifdef _MSC_VER +#pragma warning (disable: 4127) // condition expression is constant +#pragma warning (disable: 4996) // 'This function or variable may be unsafe': strcpy, strdup, sprintf, vsnprintf, sscanf, fopen +#pragma warning (disable: 26451) // [Static Analyzer] Arithmetic overflow : Using operator 'xxx' on a 4 byte value and then casting the result to an 8 byte value. Cast the value to the wider type before calling operator 'xxx' to avoid overflow(io.2). +#endif + +// Clang/GCC warnings with -Weverything +#if defined(__clang__) +#if __has_warning("-Wunknown-warning-option") +#pragma clang diagnostic ignored "-Wunknown-warning-option" // warning: unknown warning group 'xxx' // not all warnings are known by all Clang versions and they tend to be rename-happy.. so ignoring warnings triggers new warnings on some configuration. Great! +#endif +#pragma clang diagnostic ignored "-Wunknown-pragmas" // warning: unknown warning group 'xxx' +#pragma clang diagnostic ignored "-Wold-style-cast" // warning: use of old-style cast // yes, they are more terse. +#pragma clang diagnostic ignored "-Wdeprecated-declarations" // warning: 'xx' is deprecated: The POSIX name for this.. // for strdup used in demo code (so user can copy & paste the code) +#pragma clang diagnostic ignored "-Wint-to-void-pointer-cast" // warning: cast to 'void *' from smaller integer type +#pragma clang diagnostic ignored "-Wformat-security" // warning: format string is not a string literal +#pragma clang diagnostic ignored "-Wexit-time-destructors" // warning: declaration requires an exit-time destructor // exit-time destruction order is undefined. if MemFree() leads to users code that has been disabled before exit it might cause problems. ImGui coding style welcomes static/globals. +#pragma clang diagnostic ignored "-Wunused-macros" // warning: macro is not used // we define snprintf/vsnprintf on Windows so they are available, but not always used. +#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant" // warning: zero as null pointer constant // some standard header variations use #define NULL 0 +#pragma clang diagnostic ignored "-Wdouble-promotion" // warning: implicit conversion from 'float' to 'double' when passing argument to function // using printf() is a misery with this as C++ va_arg ellipsis changes float to double. +#pragma clang diagnostic ignored "-Wreserved-id-macro" // warning: macro name is a reserved identifier +#pragma clang diagnostic ignored "-Wimplicit-int-float-conversion" // warning: implicit conversion from 'xxx' to 'float' may lose precision +#elif defined(__GNUC__) +#pragma GCC diagnostic ignored "-Wpragmas" // warning: unknown option after '#pragma GCC diagnostic' kind +#pragma GCC diagnostic ignored "-Wint-to-pointer-cast" // warning: cast to pointer from integer of different size +#pragma GCC diagnostic ignored "-Wformat-security" // warning: format string is not a string literal (potentially insecure) +#pragma GCC diagnostic ignored "-Wdouble-promotion" // warning: implicit conversion from 'float' to 'double' when passing argument to function +#pragma GCC diagnostic ignored "-Wconversion" // warning: conversion to 'xxxx' from 'xxxx' may alter its value +#pragma GCC diagnostic ignored "-Wmisleading-indentation" // [__GNUC__ >= 6] warning: this 'if' clause does not guard this statement // GCC 6.0+ only. See #883 on GitHub. +#endif + +// Play it nice with Windows users (Update: May 2018, Notepad now supports Unix-style carriage returns!) +#ifdef _WIN32 +#define IM_NEWLINE "\r\n" +#else +#define IM_NEWLINE "\n" +#endif + +// Helpers +#if defined(_MSC_VER) && !defined(snprintf) +#define snprintf _snprintf +#endif +#if defined(_MSC_VER) && !defined(vsnprintf) +#define vsnprintf _vsnprintf +#endif + +// Format specifiers for 64-bit values (hasn't been decently standardized before VS2013) +#if !defined(PRId64) && defined(_MSC_VER) +#define PRId64 "I64d" +#define PRIu64 "I64u" +#elif !defined(PRId64) +#define PRId64 "lld" +#define PRIu64 "llu" +#endif + +// Helpers macros +// We normally try to not use many helpers in imgui_demo.cpp in order to make code easier to copy and paste, +// but making an exception here as those are largely simplifying code... +// In other imgui sources we can use nicer internal functions from imgui_internal.h (ImMin/ImMax) but not in the demo. +#define IM_MIN(A, B) (((A) < (B)) ? (A) : (B)) +#define IM_MAX(A, B) (((A) >= (B)) ? (A) : (B)) +#define IM_CLAMP(V, MN, MX) ((V) < (MN) ? (MN) : (V) > (MX) ? (MX) : (V)) + +// Enforce cdecl calling convention for functions called by the standard library, in case compilation settings changed the default to e.g. __vectorcall +#ifndef IMGUI_CDECL +#ifdef _MSC_VER +#define IMGUI_CDECL __cdecl +#else +#define IMGUI_CDECL +#endif +#endif + +//----------------------------------------------------------------------------- +// [SECTION] Forward Declarations, Helpers +//----------------------------------------------------------------------------- + +#if !defined(IMGUI_DISABLE_DEMO_WINDOWS) + +// Forward Declarations +static void ShowExampleAppMainMenuBar(); +static void ShowExampleAppConsole(bool* p_open); +static void ShowExampleAppCustomRendering(bool* p_open); +static void ShowExampleAppDockSpace(bool* p_open); +static void ShowExampleAppDocuments(bool* p_open); +static void ShowExampleAppLog(bool* p_open); +static void ShowExampleAppLayout(bool* p_open); +static void ShowExampleAppPropertyEditor(bool* p_open); +static void ShowExampleAppSimpleOverlay(bool* p_open); +static void ShowExampleAppAutoResize(bool* p_open); +static void ShowExampleAppConstrainedResize(bool* p_open); +static void ShowExampleAppFullscreen(bool* p_open); +static void ShowExampleAppLongText(bool* p_open); +static void ShowExampleAppWindowTitles(bool* p_open); +static void ShowExampleMenuFile(); + +// We split the contents of the big ShowDemoWindow() function into smaller functions +// (because the link time of very large functions grow non-linearly) +static void ShowDemoWindowWidgets(); +static void ShowDemoWindowLayout(); +static void ShowDemoWindowPopups(); +static void ShowDemoWindowTables(); +static void ShowDemoWindowColumns(); +static void ShowDemoWindowInputs(); + +//----------------------------------------------------------------------------- +// [SECTION] Helpers +//----------------------------------------------------------------------------- + +// Helper to display a little (?) mark which shows a tooltip when hovered. +// In your own code you may want to display an actual icon if you are using a merged icon fonts (see docs/FONTS.md) +static void HelpMarker(const char* desc) +{ + ImGui::TextDisabled("(?)"); + if (ImGui::BeginItemTooltip()) + { + ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f); + ImGui::TextUnformatted(desc); + ImGui::PopTextWrapPos(); + ImGui::EndTooltip(); + } +} + +static void ShowDockingDisabledMessage() +{ + ImGuiIO& io = ImGui::GetIO(); + ImGui::Text("ERROR: Docking is not enabled! See Demo > Configuration."); + ImGui::Text("Set io.ConfigFlags |= ImGuiConfigFlags_DockingEnable in your code, or "); + ImGui::SameLine(0.0f, 0.0f); + if (ImGui::SmallButton("click here")) + io.ConfigFlags |= ImGuiConfigFlags_DockingEnable; +} + +// Helper to wire demo markers located in code to an interactive browser +typedef void (*ImGuiDemoMarkerCallback)(const char* file, int line, const char* section, void* user_data); +extern ImGuiDemoMarkerCallback GImGuiDemoMarkerCallback; +extern void* GImGuiDemoMarkerCallbackUserData; +ImGuiDemoMarkerCallback GImGuiDemoMarkerCallback = NULL; +void* GImGuiDemoMarkerCallbackUserData = NULL; +#define IMGUI_DEMO_MARKER(section) do { if (GImGuiDemoMarkerCallback != NULL) GImGuiDemoMarkerCallback(__FILE__, __LINE__, section, GImGuiDemoMarkerCallbackUserData); } while (0) + +//----------------------------------------------------------------------------- +// [SECTION] Demo Window / ShowDemoWindow() +//----------------------------------------------------------------------------- +// - ShowDemoWindow() +// - ShowDemoWindowWidgets() +// - ShowDemoWindowLayout() +// - ShowDemoWindowPopups() +// - ShowDemoWindowTables() +// - ShowDemoWindowColumns() +// - ShowDemoWindowInputs() +//----------------------------------------------------------------------------- + +// Demonstrate most Dear ImGui features (this is big function!) +// You may execute this function to experiment with the UI and understand what it does. +// You may then search for keywords in the code when you are interested by a specific feature. +void ImGui::ShowDemoWindow(bool* p_open) +{ + // Exceptionally add an extra assert here for people confused about initial Dear ImGui setup + // Most functions would normally just assert/crash if the context is missing. + IM_ASSERT(ImGui::GetCurrentContext() != NULL && "Missing Dear ImGui context. Refer to examples app!"); + + // Examples Apps (accessible from the "Examples" menu) + static bool show_app_main_menu_bar = false; + static bool show_app_console = false; + static bool show_app_custom_rendering = false; + static bool show_app_dockspace = false; + static bool show_app_documents = false; + static bool show_app_log = false; + static bool show_app_layout = false; + static bool show_app_property_editor = false; + static bool show_app_simple_overlay = false; + static bool show_app_auto_resize = false; + static bool show_app_constrained_resize = false; + static bool show_app_fullscreen = false; + static bool show_app_long_text = false; + static bool show_app_window_titles = false; + + if (show_app_main_menu_bar) ShowExampleAppMainMenuBar(); + if (show_app_dockspace) ShowExampleAppDockSpace(&show_app_dockspace); // Process the Docking app first, as explicit DockSpace() nodes needs to be submitted early (read comments near the DockSpace function) + if (show_app_documents) ShowExampleAppDocuments(&show_app_documents); // Process the Document app next, as it may also use a DockSpace() + if (show_app_console) ShowExampleAppConsole(&show_app_console); + if (show_app_custom_rendering) ShowExampleAppCustomRendering(&show_app_custom_rendering); + if (show_app_log) ShowExampleAppLog(&show_app_log); + if (show_app_layout) ShowExampleAppLayout(&show_app_layout); + if (show_app_property_editor) ShowExampleAppPropertyEditor(&show_app_property_editor); + if (show_app_simple_overlay) ShowExampleAppSimpleOverlay(&show_app_simple_overlay); + if (show_app_auto_resize) ShowExampleAppAutoResize(&show_app_auto_resize); + if (show_app_constrained_resize) ShowExampleAppConstrainedResize(&show_app_constrained_resize); + if (show_app_fullscreen) ShowExampleAppFullscreen(&show_app_fullscreen); + if (show_app_long_text) ShowExampleAppLongText(&show_app_long_text); + if (show_app_window_titles) ShowExampleAppWindowTitles(&show_app_window_titles); + + // Dear ImGui Tools (accessible from the "Tools" menu) + static bool show_tool_metrics = false; + static bool show_tool_debug_log = false; + static bool show_tool_stack_tool = false; + static bool show_tool_style_editor = false; + static bool show_tool_about = false; + + if (show_tool_metrics) + ImGui::ShowMetricsWindow(&show_tool_metrics); + if (show_tool_debug_log) + ImGui::ShowDebugLogWindow(&show_tool_debug_log); + if (show_tool_stack_tool) + ImGui::ShowStackToolWindow(&show_tool_stack_tool); + if (show_tool_style_editor) + { + ImGui::Begin("Dear ImGui Style Editor", &show_tool_style_editor); + ImGui::ShowStyleEditor(); + ImGui::End(); + } + if (show_tool_about) + ImGui::ShowAboutWindow(&show_tool_about); + + // Demonstrate the various window flags. Typically you would just use the default! + static bool no_titlebar = false; + static bool no_scrollbar = false; + static bool no_menu = false; + static bool no_move = false; + static bool no_resize = false; + static bool no_collapse = false; + static bool no_close = false; + static bool no_nav = false; + static bool no_background = false; + static bool no_bring_to_front = false; + static bool no_docking = false; + static bool unsaved_document = false; + + ImGuiWindowFlags window_flags = 0; + if (no_titlebar) window_flags |= ImGuiWindowFlags_NoTitleBar; + if (no_scrollbar) window_flags |= ImGuiWindowFlags_NoScrollbar; + if (!no_menu) window_flags |= ImGuiWindowFlags_MenuBar; + if (no_move) window_flags |= ImGuiWindowFlags_NoMove; + if (no_resize) window_flags |= ImGuiWindowFlags_NoResize; + if (no_collapse) window_flags |= ImGuiWindowFlags_NoCollapse; + if (no_nav) window_flags |= ImGuiWindowFlags_NoNav; + if (no_background) window_flags |= ImGuiWindowFlags_NoBackground; + if (no_bring_to_front) window_flags |= ImGuiWindowFlags_NoBringToFrontOnFocus; + if (no_docking) window_flags |= ImGuiWindowFlags_NoDocking; + if (unsaved_document) window_flags |= ImGuiWindowFlags_UnsavedDocument; + if (no_close) p_open = NULL; // Don't pass our bool* to Begin + + // We specify a default position/size in case there's no data in the .ini file. + // We only do it to make the demo applications a little more welcoming, but typically this isn't required. + const ImGuiViewport* main_viewport = ImGui::GetMainViewport(); + ImGui::SetNextWindowPos(ImVec2(main_viewport->WorkPos.x + 650, main_viewport->WorkPos.y + 20), ImGuiCond_FirstUseEver); + ImGui::SetNextWindowSize(ImVec2(550, 680), ImGuiCond_FirstUseEver); + + // Main body of the Demo window starts here. + if (!ImGui::Begin("Dear ImGui Demo", p_open, window_flags)) + { + // Early out if the window is collapsed, as an optimization. + ImGui::End(); + return; + } + + // Most "big" widgets share a common width settings by default. See 'Demo->Layout->Widgets Width' for details. + // e.g. Use 2/3 of the space for widgets and 1/3 for labels (right align) + //ImGui::PushItemWidth(-ImGui::GetWindowWidth() * 0.35f); + // e.g. Leave a fixed amount of width for labels (by passing a negative value), the rest goes to widgets. + ImGui::PushItemWidth(ImGui::GetFontSize() * -12); + + // Menu Bar + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("Menu")) + { + IMGUI_DEMO_MARKER("Menu/File"); + ShowExampleMenuFile(); + ImGui::EndMenu(); + } + if (ImGui::BeginMenu("Examples")) + { + IMGUI_DEMO_MARKER("Menu/Examples"); + ImGui::MenuItem("Main menu bar", NULL, &show_app_main_menu_bar); + + ImGui::SeparatorText("Mini apps"); + ImGui::MenuItem("Console", NULL, &show_app_console); + ImGui::MenuItem("Custom rendering", NULL, &show_app_custom_rendering); + ImGui::MenuItem("Dockspace", NULL, &show_app_dockspace); + ImGui::MenuItem("Documents", NULL, &show_app_documents); + ImGui::MenuItem("Log", NULL, &show_app_log); + ImGui::MenuItem("Property editor", NULL, &show_app_property_editor); + ImGui::MenuItem("Simple layout", NULL, &show_app_layout); + ImGui::MenuItem("Simple overlay", NULL, &show_app_simple_overlay); + + ImGui::SeparatorText("Concepts"); + ImGui::MenuItem("Auto-resizing window", NULL, &show_app_auto_resize); + ImGui::MenuItem("Constrained-resizing window", NULL, &show_app_constrained_resize); + ImGui::MenuItem("Fullscreen window", NULL, &show_app_fullscreen); + ImGui::MenuItem("Long text display", NULL, &show_app_long_text); + ImGui::MenuItem("Manipulating window titles", NULL, &show_app_window_titles); + + ImGui::EndMenu(); + } + //if (ImGui::MenuItem("MenuItem")) {} // You can also use MenuItem() inside a menu bar! + if (ImGui::BeginMenu("Tools")) + { + IMGUI_DEMO_MARKER("Menu/Tools"); +#ifndef IMGUI_DISABLE_DEBUG_TOOLS + const bool has_debug_tools = true; +#else + const bool has_debug_tools = false; +#endif + ImGui::MenuItem("Metrics/Debugger", NULL, &show_tool_metrics, has_debug_tools); + ImGui::MenuItem("Debug Log", NULL, &show_tool_debug_log, has_debug_tools); + ImGui::MenuItem("Stack Tool", NULL, &show_tool_stack_tool, has_debug_tools); + ImGui::MenuItem("Style Editor", NULL, &show_tool_style_editor); + ImGui::MenuItem("About Dear ImGui", NULL, &show_tool_about); + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + + ImGui::Text("dear imgui says hello! (%s) (%d)", IMGUI_VERSION, IMGUI_VERSION_NUM); + ImGui::Spacing(); + + IMGUI_DEMO_MARKER("Help"); + if (ImGui::CollapsingHeader("Help")) + { + ImGui::SeparatorText("ABOUT THIS DEMO:"); + ImGui::BulletText("Sections below are demonstrating many aspects of the library."); + ImGui::BulletText("The \"Examples\" menu above leads to more demo contents."); + ImGui::BulletText("The \"Tools\" menu above gives access to: About Box, Style Editor,\n" + "and Metrics/Debugger (general purpose Dear ImGui debugging tool)."); + + ImGui::SeparatorText("PROGRAMMER GUIDE:"); + ImGui::BulletText("See the ShowDemoWindow() code in imgui_demo.cpp. <- you are here!"); + ImGui::BulletText("See comments in imgui.cpp."); + ImGui::BulletText("See example applications in the examples/ folder."); + ImGui::BulletText("Read the FAQ at https://www.dearimgui.com/faq/"); + ImGui::BulletText("Set 'io.ConfigFlags |= NavEnableKeyboard' for keyboard controls."); + ImGui::BulletText("Set 'io.ConfigFlags |= NavEnableGamepad' for gamepad controls."); + + ImGui::SeparatorText("USER GUIDE:"); + ImGui::ShowUserGuide(); + } + + IMGUI_DEMO_MARKER("Configuration"); + if (ImGui::CollapsingHeader("Configuration")) + { + ImGuiIO& io = ImGui::GetIO(); + + if (ImGui::TreeNode("Configuration##2")) + { + ImGui::SeparatorText("General"); + ImGui::CheckboxFlags("io.ConfigFlags: NavEnableKeyboard", &io.ConfigFlags, ImGuiConfigFlags_NavEnableKeyboard); + ImGui::SameLine(); HelpMarker("Enable keyboard controls."); + ImGui::CheckboxFlags("io.ConfigFlags: NavEnableGamepad", &io.ConfigFlags, ImGuiConfigFlags_NavEnableGamepad); + ImGui::SameLine(); HelpMarker("Enable gamepad controls. Require backend to set io.BackendFlags |= ImGuiBackendFlags_HasGamepad.\n\nRead instructions in imgui.cpp for details."); + ImGui::CheckboxFlags("io.ConfigFlags: NavEnableSetMousePos", &io.ConfigFlags, ImGuiConfigFlags_NavEnableSetMousePos); + ImGui::SameLine(); HelpMarker("Instruct navigation to move the mouse cursor. See comment for ImGuiConfigFlags_NavEnableSetMousePos."); + ImGui::CheckboxFlags("io.ConfigFlags: NoMouse", &io.ConfigFlags, ImGuiConfigFlags_NoMouse); + if (io.ConfigFlags & ImGuiConfigFlags_NoMouse) + { + // The "NoMouse" option can get us stuck with a disabled mouse! Let's provide an alternative way to fix it: + if (fmodf((float)ImGui::GetTime(), 0.40f) < 0.20f) + { + ImGui::SameLine(); + ImGui::Text("<>"); + } + if (ImGui::IsKeyPressed(ImGuiKey_Space)) + io.ConfigFlags &= ~ImGuiConfigFlags_NoMouse; + } + ImGui::CheckboxFlags("io.ConfigFlags: NoMouseCursorChange", &io.ConfigFlags, ImGuiConfigFlags_NoMouseCursorChange); + ImGui::SameLine(); HelpMarker("Instruct backend to not alter mouse cursor shape and visibility."); + + ImGui::CheckboxFlags("io.ConfigFlags: DockingEnable", &io.ConfigFlags, ImGuiConfigFlags_DockingEnable); + ImGui::SameLine(); + if (io.ConfigDockingWithShift) + HelpMarker("Drag from window title bar or their tab to dock/undock. Hold SHIFT to enable docking.\n\nDrag from window menu button (upper-left button) to undock an entire node (all windows)."); + else + HelpMarker("Drag from window title bar or their tab to dock/undock. Hold SHIFT to disable docking.\n\nDrag from window menu button (upper-left button) to undock an entire node (all windows)."); + if (io.ConfigFlags & ImGuiConfigFlags_DockingEnable) + { + ImGui::Indent(); + ImGui::Checkbox("io.ConfigDockingNoSplit", &io.ConfigDockingNoSplit); + ImGui::SameLine(); HelpMarker("Simplified docking mode: disable window splitting, so docking is limited to merging multiple windows together into tab-bars."); + ImGui::Checkbox("io.ConfigDockingWithShift", &io.ConfigDockingWithShift); + ImGui::SameLine(); HelpMarker("Enable docking when holding Shift only (allow to drop in wider space, reduce visual noise)"); + ImGui::Checkbox("io.ConfigDockingAlwaysTabBar", &io.ConfigDockingAlwaysTabBar); + ImGui::SameLine(); HelpMarker("Create a docking node and tab-bar on single floating windows."); + ImGui::Checkbox("io.ConfigDockingTransparentPayload", &io.ConfigDockingTransparentPayload); + ImGui::SameLine(); HelpMarker("Make window or viewport transparent when docking and only display docking boxes on the target viewport. Useful if rendering of multiple viewport cannot be synced. Best used with ConfigViewportsNoAutoMerge."); + ImGui::Unindent(); + } + + ImGui::CheckboxFlags("io.ConfigFlags: ViewportsEnable", &io.ConfigFlags, ImGuiConfigFlags_ViewportsEnable); + ImGui::SameLine(); HelpMarker("[beta] Enable beta multi-viewports support. See ImGuiPlatformIO for details."); + if (io.ConfigFlags & ImGuiConfigFlags_ViewportsEnable) + { + ImGui::Indent(); + ImGui::Checkbox("io.ConfigViewportsNoAutoMerge", &io.ConfigViewportsNoAutoMerge); + ImGui::SameLine(); HelpMarker("Set to make all floating imgui windows always create their own viewport. Otherwise, they are merged into the main host viewports when overlapping it."); + ImGui::Checkbox("io.ConfigViewportsNoTaskBarIcon", &io.ConfigViewportsNoTaskBarIcon); + ImGui::SameLine(); HelpMarker("Toggling this at runtime is normally unsupported (most platform backends won't refresh the task bar icon state right away)."); + ImGui::Checkbox("io.ConfigViewportsNoDecoration", &io.ConfigViewportsNoDecoration); + ImGui::SameLine(); HelpMarker("Toggling this at runtime is normally unsupported (most platform backends won't refresh the decoration right away)."); + ImGui::Checkbox("io.ConfigViewportsNoDefaultParent", &io.ConfigViewportsNoDefaultParent); + ImGui::SameLine(); HelpMarker("Toggling this at runtime is normally unsupported (most platform backends won't refresh the parenting right away)."); + ImGui::Unindent(); + } + + ImGui::Checkbox("io.ConfigInputTrickleEventQueue", &io.ConfigInputTrickleEventQueue); + ImGui::SameLine(); HelpMarker("Enable input queue trickling: some types of events submitted during the same frame (e.g. button down + up) will be spread over multiple frames, improving interactions with low framerates."); + ImGui::Checkbox("io.MouseDrawCursor", &io.MouseDrawCursor); + ImGui::SameLine(); HelpMarker("Instruct Dear ImGui to render a mouse cursor itself. Note that a mouse cursor rendered via your application GPU rendering path will feel more laggy than hardware cursor, but will be more in sync with your other visuals.\n\nSome desktop applications may use both kinds of cursors (e.g. enable software cursor only when resizing/dragging something)."); + + ImGui::SeparatorText("Widgets"); + ImGui::Checkbox("io.ConfigInputTextCursorBlink", &io.ConfigInputTextCursorBlink); + ImGui::SameLine(); HelpMarker("Enable blinking cursor (optional as some users consider it to be distracting)."); + ImGui::Checkbox("io.ConfigInputTextEnterKeepActive", &io.ConfigInputTextEnterKeepActive); + ImGui::SameLine(); HelpMarker("Pressing Enter will keep item active and select contents (single-line only)."); + ImGui::Checkbox("io.ConfigDragClickToInputText", &io.ConfigDragClickToInputText); + ImGui::SameLine(); HelpMarker("Enable turning DragXXX widgets into text input with a simple mouse click-release (without moving)."); + ImGui::Checkbox("io.ConfigWindowsResizeFromEdges", &io.ConfigWindowsResizeFromEdges); + ImGui::SameLine(); HelpMarker("Enable resizing of windows from their edges and from the lower-left corner.\nThis requires (io.BackendFlags & ImGuiBackendFlags_HasMouseCursors) because it needs mouse cursor feedback."); + ImGui::Checkbox("io.ConfigWindowsMoveFromTitleBarOnly", &io.ConfigWindowsMoveFromTitleBarOnly); + ImGui::Checkbox("io.ConfigMacOSXBehaviors", &io.ConfigMacOSXBehaviors); + ImGui::Text("Also see Style->Rendering for rendering options."); + + ImGui::SeparatorText("Debug"); + ImGui::BeginDisabled(); + ImGui::Checkbox("io.ConfigDebugBeginReturnValueOnce", &io.ConfigDebugBeginReturnValueOnce); // . + ImGui::EndDisabled(); + ImGui::SameLine(); HelpMarker("First calls to Begin()/BeginChild() will return false.\n\nTHIS OPTION IS DISABLED because it needs to be set at application boot-time to make sense. Showing the disabled option is a way to make this feature easier to discover"); + ImGui::Checkbox("io.ConfigDebugBeginReturnValueLoop", &io.ConfigDebugBeginReturnValueLoop); + ImGui::SameLine(); HelpMarker("Some calls to Begin()/BeginChild() will return false.\n\nWill cycle through window depths then repeat. Windows should be flickering while running."); + ImGui::Checkbox("io.ConfigDebugIgnoreFocusLoss", &io.ConfigDebugIgnoreFocusLoss); + ImGui::SameLine(); HelpMarker("Option to deactivate io.AddFocusEvent(false) handling. May facilitate interactions with a debugger when focus loss leads to clearing inputs data."); + ImGui::Checkbox("io.ConfigDebugIniSettings", &io.ConfigDebugIniSettings); + ImGui::SameLine(); HelpMarker("Option to save .ini data with extra comments (particularly helpful for Docking, but makes saving slower)."); + + ImGui::TreePop(); + ImGui::Spacing(); + } + + IMGUI_DEMO_MARKER("Configuration/Backend Flags"); + if (ImGui::TreeNode("Backend Flags")) + { + HelpMarker( + "Those flags are set by the backends (imgui_impl_xxx files) to specify their capabilities.\n" + "Here we expose them as read-only fields to avoid breaking interactions with your backend."); + + // Make a local copy to avoid modifying actual backend flags. + // FIXME: Maybe we need a BeginReadonly() equivalent to keep label bright? + ImGui::BeginDisabled(); + ImGui::CheckboxFlags("io.BackendFlags: HasGamepad", &io.BackendFlags, ImGuiBackendFlags_HasGamepad); + ImGui::CheckboxFlags("io.BackendFlags: HasMouseCursors", &io.BackendFlags, ImGuiBackendFlags_HasMouseCursors); + ImGui::CheckboxFlags("io.BackendFlags: HasSetMousePos", &io.BackendFlags, ImGuiBackendFlags_HasSetMousePos); + ImGui::CheckboxFlags("io.BackendFlags: PlatformHasViewports", &io.BackendFlags, ImGuiBackendFlags_PlatformHasViewports); + ImGui::CheckboxFlags("io.BackendFlags: HasMouseHoveredViewport",&io.BackendFlags, ImGuiBackendFlags_HasMouseHoveredViewport); + ImGui::CheckboxFlags("io.BackendFlags: RendererHasVtxOffset", &io.BackendFlags, ImGuiBackendFlags_RendererHasVtxOffset); + ImGui::CheckboxFlags("io.BackendFlags: RendererHasViewports", &io.BackendFlags, ImGuiBackendFlags_RendererHasViewports); + ImGui::EndDisabled(); + ImGui::TreePop(); + ImGui::Spacing(); + } + + IMGUI_DEMO_MARKER("Configuration/Style"); + if (ImGui::TreeNode("Style")) + { + HelpMarker("The same contents can be accessed in 'Tools->Style Editor' or by calling the ShowStyleEditor() function."); + ImGui::ShowStyleEditor(); + ImGui::TreePop(); + ImGui::Spacing(); + } + + IMGUI_DEMO_MARKER("Configuration/Capture, Logging"); + if (ImGui::TreeNode("Capture/Logging")) + { + HelpMarker( + "The logging API redirects all text output so you can easily capture the content of " + "a window or a block. Tree nodes can be automatically expanded.\n" + "Try opening any of the contents below in this window and then click one of the \"Log To\" button."); + ImGui::LogButtons(); + + HelpMarker("You can also call ImGui::LogText() to output directly to the log without a visual output."); + if (ImGui::Button("Copy \"Hello, world!\" to clipboard")) + { + ImGui::LogToClipboard(); + ImGui::LogText("Hello, world!"); + ImGui::LogFinish(); + } + ImGui::TreePop(); + } + } + + IMGUI_DEMO_MARKER("Window options"); + if (ImGui::CollapsingHeader("Window options")) + { + if (ImGui::BeginTable("split", 3)) + { + ImGui::TableNextColumn(); ImGui::Checkbox("No titlebar", &no_titlebar); + ImGui::TableNextColumn(); ImGui::Checkbox("No scrollbar", &no_scrollbar); + ImGui::TableNextColumn(); ImGui::Checkbox("No menu", &no_menu); + ImGui::TableNextColumn(); ImGui::Checkbox("No move", &no_move); + ImGui::TableNextColumn(); ImGui::Checkbox("No resize", &no_resize); + ImGui::TableNextColumn(); ImGui::Checkbox("No collapse", &no_collapse); + ImGui::TableNextColumn(); ImGui::Checkbox("No close", &no_close); + ImGui::TableNextColumn(); ImGui::Checkbox("No nav", &no_nav); + ImGui::TableNextColumn(); ImGui::Checkbox("No background", &no_background); + ImGui::TableNextColumn(); ImGui::Checkbox("No bring to front", &no_bring_to_front); + ImGui::TableNextColumn(); ImGui::Checkbox("No docking", &no_docking); + ImGui::TableNextColumn(); ImGui::Checkbox("Unsaved document", &unsaved_document); + ImGui::EndTable(); + } + } + + // All demo contents + ShowDemoWindowWidgets(); + ShowDemoWindowLayout(); + ShowDemoWindowPopups(); + ShowDemoWindowTables(); + ShowDemoWindowInputs(); + + // End of ShowDemoWindow() + ImGui::PopItemWidth(); + ImGui::End(); +} + +static void ShowDemoWindowWidgets() +{ + IMGUI_DEMO_MARKER("Widgets"); + if (!ImGui::CollapsingHeader("Widgets")) + return; + + static bool disable_all = false; // The Checkbox for that is inside the "Disabled" section at the bottom + if (disable_all) + ImGui::BeginDisabled(); + + IMGUI_DEMO_MARKER("Widgets/Basic"); + if (ImGui::TreeNode("Basic")) + { + ImGui::SeparatorText("General"); + + IMGUI_DEMO_MARKER("Widgets/Basic/Button"); + static int clicked = 0; + if (ImGui::Button("Button")) + clicked++; + if (clicked & 1) + { + ImGui::SameLine(); + ImGui::Text("Thanks for clicking me!"); + } + + IMGUI_DEMO_MARKER("Widgets/Basic/Checkbox"); + static bool check = true; + ImGui::Checkbox("checkbox", &check); + + IMGUI_DEMO_MARKER("Widgets/Basic/RadioButton"); + static int e = 0; + ImGui::RadioButton("radio a", &e, 0); ImGui::SameLine(); + ImGui::RadioButton("radio b", &e, 1); ImGui::SameLine(); + ImGui::RadioButton("radio c", &e, 2); + + // Color buttons, demonstrate using PushID() to add unique identifier in the ID stack, and changing style. + IMGUI_DEMO_MARKER("Widgets/Basic/Buttons (Colored)"); + for (int i = 0; i < 7; i++) + { + if (i > 0) + ImGui::SameLine(); + ImGui::PushID(i); + ImGui::PushStyleColor(ImGuiCol_Button, (ImVec4)ImColor::HSV(i / 7.0f, 0.6f, 0.6f)); + ImGui::PushStyleColor(ImGuiCol_ButtonHovered, (ImVec4)ImColor::HSV(i / 7.0f, 0.7f, 0.7f)); + ImGui::PushStyleColor(ImGuiCol_ButtonActive, (ImVec4)ImColor::HSV(i / 7.0f, 0.8f, 0.8f)); + ImGui::Button("Click"); + ImGui::PopStyleColor(3); + ImGui::PopID(); + } + + // Use AlignTextToFramePadding() to align text baseline to the baseline of framed widgets elements + // (otherwise a Text+SameLine+Button sequence will have the text a little too high by default!) + // See 'Demo->Layout->Text Baseline Alignment' for details. + ImGui::AlignTextToFramePadding(); + ImGui::Text("Hold to repeat:"); + ImGui::SameLine(); + + // Arrow buttons with Repeater + IMGUI_DEMO_MARKER("Widgets/Basic/Buttons (Repeating)"); + static int counter = 0; + float spacing = ImGui::GetStyle().ItemInnerSpacing.x; + ImGui::PushButtonRepeat(true); + if (ImGui::ArrowButton("##left", ImGuiDir_Left)) { counter--; } + ImGui::SameLine(0.0f, spacing); + if (ImGui::ArrowButton("##right", ImGuiDir_Right)) { counter++; } + ImGui::PopButtonRepeat(); + ImGui::SameLine(); + ImGui::Text("%d", counter); + + ImGui::Button("Tooltip"); + ImGui::SetItemTooltip("I am a tooltip"); + + ImGui::LabelText("label", "Value"); + + ImGui::SeparatorText("Inputs"); + + { + // To wire InputText() with std::string or any other custom string type, + // see the "Text Input > Resize Callback" section of this demo, and the misc/cpp/imgui_stdlib.h file. + IMGUI_DEMO_MARKER("Widgets/Basic/InputText"); + static char str0[128] = "Hello, world!"; + ImGui::InputText("input text", str0, IM_ARRAYSIZE(str0)); + ImGui::SameLine(); HelpMarker( + "USER:\n" + "Hold SHIFT or use mouse to select text.\n" + "CTRL+Left/Right to word jump.\n" + "CTRL+A or Double-Click to select all.\n" + "CTRL+X,CTRL+C,CTRL+V clipboard.\n" + "CTRL+Z,CTRL+Y undo/redo.\n" + "ESCAPE to revert.\n\n" + "PROGRAMMER:\n" + "You can use the ImGuiInputTextFlags_CallbackResize facility if you need to wire InputText() " + "to a dynamic string type. See misc/cpp/imgui_stdlib.h for an example (this is not demonstrated " + "in imgui_demo.cpp)."); + + static char str1[128] = ""; + ImGui::InputTextWithHint("input text (w/ hint)", "enter text here", str1, IM_ARRAYSIZE(str1)); + + IMGUI_DEMO_MARKER("Widgets/Basic/InputInt, InputFloat"); + static int i0 = 123; + ImGui::InputInt("input int", &i0); + + static float f0 = 0.001f; + ImGui::InputFloat("input float", &f0, 0.01f, 1.0f, "%.3f"); + + static double d0 = 999999.00000001; + ImGui::InputDouble("input double", &d0, 0.01f, 1.0f, "%.8f"); + + static float f1 = 1.e10f; + ImGui::InputFloat("input scientific", &f1, 0.0f, 0.0f, "%e"); + ImGui::SameLine(); HelpMarker( + "You can input value using the scientific notation,\n" + " e.g. \"1e+8\" becomes \"100000000\"."); + + static float vec4a[4] = { 0.10f, 0.20f, 0.30f, 0.44f }; + ImGui::InputFloat3("input float3", vec4a); + } + + ImGui::SeparatorText("Drags"); + + { + IMGUI_DEMO_MARKER("Widgets/Basic/DragInt, DragFloat"); + static int i1 = 50, i2 = 42; + ImGui::DragInt("drag int", &i1, 1); + ImGui::SameLine(); HelpMarker( + "Click and drag to edit value.\n" + "Hold SHIFT/ALT for faster/slower edit.\n" + "Double-click or CTRL+click to input value."); + + ImGui::DragInt("drag int 0..100", &i2, 1, 0, 100, "%d%%", ImGuiSliderFlags_AlwaysClamp); + + static float f1 = 1.00f, f2 = 0.0067f; + ImGui::DragFloat("drag float", &f1, 0.005f); + ImGui::DragFloat("drag small float", &f2, 0.0001f, 0.0f, 0.0f, "%.06f ns"); + } + + ImGui::SeparatorText("Sliders"); + + { + IMGUI_DEMO_MARKER("Widgets/Basic/SliderInt, SliderFloat"); + static int i1 = 0; + ImGui::SliderInt("slider int", &i1, -1, 3); + ImGui::SameLine(); HelpMarker("CTRL+click to input value."); + + static float f1 = 0.123f, f2 = 0.0f; + ImGui::SliderFloat("slider float", &f1, 0.0f, 1.0f, "ratio = %.3f"); + ImGui::SliderFloat("slider float (log)", &f2, -10.0f, 10.0f, "%.4f", ImGuiSliderFlags_Logarithmic); + + IMGUI_DEMO_MARKER("Widgets/Basic/SliderAngle"); + static float angle = 0.0f; + ImGui::SliderAngle("slider angle", &angle); + + // Using the format string to display a name instead of an integer. + // Here we completely omit '%d' from the format string, so it'll only display a name. + // This technique can also be used with DragInt(). + IMGUI_DEMO_MARKER("Widgets/Basic/Slider (enum)"); + enum Element { Element_Fire, Element_Earth, Element_Air, Element_Water, Element_COUNT }; + static int elem = Element_Fire; + const char* elems_names[Element_COUNT] = { "Fire", "Earth", "Air", "Water" }; + const char* elem_name = (elem >= 0 && elem < Element_COUNT) ? elems_names[elem] : "Unknown"; + ImGui::SliderInt("slider enum", &elem, 0, Element_COUNT - 1, elem_name); // Use ImGuiSliderFlags_NoInput flag to disable CTRL+Click here. + ImGui::SameLine(); HelpMarker("Using the format string parameter to display a name instead of the underlying integer."); + } + + ImGui::SeparatorText("Selectors/Pickers"); + + { + IMGUI_DEMO_MARKER("Widgets/Basic/ColorEdit3, ColorEdit4"); + static float col1[3] = { 1.0f, 0.0f, 0.2f }; + static float col2[4] = { 0.4f, 0.7f, 0.0f, 0.5f }; + ImGui::ColorEdit3("color 1", col1); + ImGui::SameLine(); HelpMarker( + "Click on the color square to open a color picker.\n" + "Click and hold to use drag and drop.\n" + "Right-click on the color square to show options.\n" + "CTRL+click on individual component to input value.\n"); + + ImGui::ColorEdit4("color 2", col2); + } + + { + // Using the _simplified_ one-liner Combo() api here + // See "Combo" section for examples of how to use the more flexible BeginCombo()/EndCombo() api. + IMGUI_DEMO_MARKER("Widgets/Basic/Combo"); + const char* items[] = { "AAAA", "BBBB", "CCCC", "DDDD", "EEEE", "FFFF", "GGGG", "HHHH", "IIIIIII", "JJJJ", "KKKKKKK" }; + static int item_current = 0; + ImGui::Combo("combo", &item_current, items, IM_ARRAYSIZE(items)); + ImGui::SameLine(); HelpMarker( + "Using the simplified one-liner Combo API here.\nRefer to the \"Combo\" section below for an explanation of how to use the more flexible and general BeginCombo/EndCombo API."); + } + + { + // Using the _simplified_ one-liner ListBox() api here + // See "List boxes" section for examples of how to use the more flexible BeginListBox()/EndListBox() api. + IMGUI_DEMO_MARKER("Widgets/Basic/ListBox"); + const char* items[] = { "Apple", "Banana", "Cherry", "Kiwi", "Mango", "Orange", "Pineapple", "Strawberry", "Watermelon" }; + static int item_current = 1; + ImGui::ListBox("listbox", &item_current, items, IM_ARRAYSIZE(items), 4); + ImGui::SameLine(); HelpMarker( + "Using the simplified one-liner ListBox API here.\nRefer to the \"List boxes\" section below for an explanation of how to use the more flexible and general BeginListBox/EndListBox API."); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Tooltips"); + if (ImGui::TreeNode("Tooltips")) + { + // Tooltips are windows following the mouse. They do not take focus away. + ImGui::SeparatorText("General"); + + // Typical use cases: + // - Short-form (text only): SetItemTooltip("Hello"); + // - Short-form (any contents): if (BeginItemTooltip()) { Text("Hello"); EndTooltip(); } + + // - Full-form (text only): if (IsItemHovered(...)) { SetTooltip("Hello"); } + // - Full-form (any contents): if (IsItemHovered(...) && BeginTooltip()) { Text("Hello"); EndTooltip(); } + + HelpMarker( + "Tooltip are typically created by using a IsItemHovered() + SetTooltip() sequence.\n\n" + "We provide a helper SetItemTooltip() function to perform the two with standards flags."); + + ImVec2 sz = ImVec2(-FLT_MIN, 0.0f); + + ImGui::Button("Basic", sz); + ImGui::SetItemTooltip("I am a tooltip"); + + ImGui::Button("Fancy", sz); + if (ImGui::BeginItemTooltip()) + { + ImGui::Text("I am a fancy tooltip"); + static float arr[] = { 0.6f, 0.1f, 1.0f, 0.5f, 0.92f, 0.1f, 0.2f }; + ImGui::PlotLines("Curve", arr, IM_ARRAYSIZE(arr)); + ImGui::Text("Sin(time) = %f", sinf((float)ImGui::GetTime())); + ImGui::EndTooltip(); + } + + ImGui::SeparatorText("Always On"); + + // Showcase NOT relying on a IsItemHovered() to emit a tooltip. + // Here the tooltip is always emitted when 'always_on == true'. + static int always_on = 0; + ImGui::RadioButton("Off", &always_on, 0); + ImGui::SameLine(); + ImGui::RadioButton("Always On (Simple)", &always_on, 1); + ImGui::SameLine(); + ImGui::RadioButton("Always On (Advanced)", &always_on, 2); + if (always_on == 1) + ImGui::SetTooltip("I am following you around."); + else if (always_on == 2 && ImGui::BeginTooltip()) + { + ImGui::ProgressBar(sinf((float)ImGui::GetTime()) * 0.5f + 0.5f, ImVec2(ImGui::GetFontSize() * 25, 0.0f)); + ImGui::EndTooltip(); + } + + ImGui::SeparatorText("Custom"); + + HelpMarker( + "Passing ImGuiHoveredFlags_ForTooltip to IsItemHovered() is the preferred way to standardize" + "tooltip activation details across your application. You may however decide to use custom" + "flags for a specific tooltip instance."); + + // The following examples are passed for documentation purpose but may not be useful to most users. + // Passing ImGuiHoveredFlags_Tooltip to IsItemHovered() will pull ImGuiHoveredFlags flags values from + // 'style.HoverFlagsForTooltipMouse' or 'style.HoverFlagsForTooltipNav' depending on whether mouse or gamepad/keyboard is being used. + // With default settings, ImGuiHoveredFlags_Tooltip is equivalent to ImGuiHoveredFlags_DelayShort + ImGuiHoveredFlags_Stationary. + ImGui::Button("Manual", sz); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_ForTooltip)) + ImGui::SetTooltip("I am a manually emitted tooltip."); + + ImGui::Button("DelayNone", sz); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_DelayNone)) + ImGui::SetTooltip("I am a tooltip with no delay."); + + ImGui::Button("DelayShort", sz); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_DelayShort | ImGuiHoveredFlags_NoSharedDelay)) + ImGui::SetTooltip("I am a tooltip with a short delay (%0.2f sec).", ImGui::GetStyle().HoverDelayShort); + + ImGui::Button("DelayLong", sz); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_DelayNormal | ImGuiHoveredFlags_NoSharedDelay)) + ImGui::SetTooltip("I am a tooltip with a long delay (%0.2f sec).", ImGui::GetStyle().HoverDelayNormal); + + ImGui::Button("Stationary", sz); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_Stationary)) + ImGui::SetTooltip("I am a tooltip requiring mouse to be stationary before activating."); + + // Using ImGuiHoveredFlags_ForTooltip will pull flags from 'style.HoverFlagsForTooltipMouse' or 'style.HoverFlagsForTooltipNav', + // which default value include the ImGuiHoveredFlags_AllowWhenDisabled flag. + // As a result, Set + ImGui::BeginDisabled(); + ImGui::Button("Disabled item", sz); + ImGui::EndDisabled(); + if (ImGui::IsItemHovered(ImGuiHoveredFlags_ForTooltip)) + ImGui::SetTooltip("I am a a tooltip for a disabled item."); + + ImGui::TreePop(); + } + + // Testing ImGuiOnceUponAFrame helper. + //static ImGuiOnceUponAFrame once; + //for (int i = 0; i < 5; i++) + // if (once) + // ImGui::Text("This will be displayed only once."); + + IMGUI_DEMO_MARKER("Widgets/Trees"); + if (ImGui::TreeNode("Trees")) + { + IMGUI_DEMO_MARKER("Widgets/Trees/Basic trees"); + if (ImGui::TreeNode("Basic trees")) + { + for (int i = 0; i < 5; i++) + { + // Use SetNextItemOpen() so set the default state of a node to be open. We could + // also use TreeNodeEx() with the ImGuiTreeNodeFlags_DefaultOpen flag to achieve the same thing! + if (i == 0) + ImGui::SetNextItemOpen(true, ImGuiCond_Once); + + if (ImGui::TreeNode((void*)(intptr_t)i, "Child %d", i)) + { + ImGui::Text("blah blah"); + ImGui::SameLine(); + if (ImGui::SmallButton("button")) {} + ImGui::TreePop(); + } + } + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Trees/Advanced, with Selectable nodes"); + if (ImGui::TreeNode("Advanced, with Selectable nodes")) + { + HelpMarker( + "This is a more typical looking tree with selectable nodes.\n" + "Click to select, CTRL+Click to toggle, click on arrows or double-click to open."); + static ImGuiTreeNodeFlags base_flags = ImGuiTreeNodeFlags_OpenOnArrow | ImGuiTreeNodeFlags_OpenOnDoubleClick | ImGuiTreeNodeFlags_SpanAvailWidth; + static bool align_label_with_current_x_position = false; + static bool test_drag_and_drop = false; + ImGui::CheckboxFlags("ImGuiTreeNodeFlags_OpenOnArrow", &base_flags, ImGuiTreeNodeFlags_OpenOnArrow); + ImGui::CheckboxFlags("ImGuiTreeNodeFlags_OpenOnDoubleClick", &base_flags, ImGuiTreeNodeFlags_OpenOnDoubleClick); + ImGui::CheckboxFlags("ImGuiTreeNodeFlags_SpanAvailWidth", &base_flags, ImGuiTreeNodeFlags_SpanAvailWidth); ImGui::SameLine(); HelpMarker("Extend hit area to all available width instead of allowing more items to be laid out after the node."); + ImGui::CheckboxFlags("ImGuiTreeNodeFlags_SpanFullWidth", &base_flags, ImGuiTreeNodeFlags_SpanFullWidth); + ImGui::Checkbox("Align label with current X position", &align_label_with_current_x_position); + ImGui::Checkbox("Test tree node as drag source", &test_drag_and_drop); + ImGui::Text("Hello!"); + if (align_label_with_current_x_position) + ImGui::Unindent(ImGui::GetTreeNodeToLabelSpacing()); + + // 'selection_mask' is dumb representation of what may be user-side selection state. + // You may retain selection state inside or outside your objects in whatever format you see fit. + // 'node_clicked' is temporary storage of what node we have clicked to process selection at the end + /// of the loop. May be a pointer to your own node type, etc. + static int selection_mask = (1 << 2); + int node_clicked = -1; + for (int i = 0; i < 6; i++) + { + // Disable the default "open on single-click behavior" + set Selected flag according to our selection. + // To alter selection we use IsItemClicked() && !IsItemToggledOpen(), so clicking on an arrow doesn't alter selection. + ImGuiTreeNodeFlags node_flags = base_flags; + const bool is_selected = (selection_mask & (1 << i)) != 0; + if (is_selected) + node_flags |= ImGuiTreeNodeFlags_Selected; + if (i < 3) + { + // Items 0..2 are Tree Node + bool node_open = ImGui::TreeNodeEx((void*)(intptr_t)i, node_flags, "Selectable Node %d", i); + if (ImGui::IsItemClicked() && !ImGui::IsItemToggledOpen()) + node_clicked = i; + if (test_drag_and_drop && ImGui::BeginDragDropSource()) + { + ImGui::SetDragDropPayload("_TREENODE", NULL, 0); + ImGui::Text("This is a drag and drop source"); + ImGui::EndDragDropSource(); + } + if (node_open) + { + ImGui::BulletText("Blah blah\nBlah Blah"); + ImGui::TreePop(); + } + } + else + { + // Items 3..5 are Tree Leaves + // The only reason we use TreeNode at all is to allow selection of the leaf. Otherwise we can + // use BulletText() or advance the cursor by GetTreeNodeToLabelSpacing() and call Text(). + node_flags |= ImGuiTreeNodeFlags_Leaf | ImGuiTreeNodeFlags_NoTreePushOnOpen; // ImGuiTreeNodeFlags_Bullet + ImGui::TreeNodeEx((void*)(intptr_t)i, node_flags, "Selectable Leaf %d", i); + if (ImGui::IsItemClicked() && !ImGui::IsItemToggledOpen()) + node_clicked = i; + if (test_drag_and_drop && ImGui::BeginDragDropSource()) + { + ImGui::SetDragDropPayload("_TREENODE", NULL, 0); + ImGui::Text("This is a drag and drop source"); + ImGui::EndDragDropSource(); + } + } + } + if (node_clicked != -1) + { + // Update selection state + // (process outside of tree loop to avoid visual inconsistencies during the clicking frame) + if (ImGui::GetIO().KeyCtrl) + selection_mask ^= (1 << node_clicked); // CTRL+click to toggle + else //if (!(selection_mask & (1 << node_clicked))) // Depending on selection behavior you want, may want to preserve selection when clicking on item that is part of the selection + selection_mask = (1 << node_clicked); // Click to single-select + } + if (align_label_with_current_x_position) + ImGui::Indent(ImGui::GetTreeNodeToLabelSpacing()); + ImGui::TreePop(); + } + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Collapsing Headers"); + if (ImGui::TreeNode("Collapsing Headers")) + { + static bool closable_group = true; + ImGui::Checkbox("Show 2nd header", &closable_group); + if (ImGui::CollapsingHeader("Header", ImGuiTreeNodeFlags_None)) + { + ImGui::Text("IsItemHovered: %d", ImGui::IsItemHovered()); + for (int i = 0; i < 5; i++) + ImGui::Text("Some content %d", i); + } + if (ImGui::CollapsingHeader("Header with a close button", &closable_group)) + { + ImGui::Text("IsItemHovered: %d", ImGui::IsItemHovered()); + for (int i = 0; i < 5; i++) + ImGui::Text("More content %d", i); + } + /* + if (ImGui::CollapsingHeader("Header with a bullet", ImGuiTreeNodeFlags_Bullet)) + ImGui::Text("IsItemHovered: %d", ImGui::IsItemHovered()); + */ + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Bullets"); + if (ImGui::TreeNode("Bullets")) + { + ImGui::BulletText("Bullet point 1"); + ImGui::BulletText("Bullet point 2\nOn multiple lines"); + if (ImGui::TreeNode("Tree node")) + { + ImGui::BulletText("Another bullet point"); + ImGui::TreePop(); + } + ImGui::Bullet(); ImGui::Text("Bullet point 3 (two calls)"); + ImGui::Bullet(); ImGui::SmallButton("Button"); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text"); + if (ImGui::TreeNode("Text")) + { + IMGUI_DEMO_MARKER("Widgets/Text/Colored Text"); + if (ImGui::TreeNode("Colorful Text")) + { + // Using shortcut. You can use PushStyleColor()/PopStyleColor() for more flexibility. + ImGui::TextColored(ImVec4(1.0f, 0.0f, 1.0f, 1.0f), "Pink"); + ImGui::TextColored(ImVec4(1.0f, 1.0f, 0.0f, 1.0f), "Yellow"); + ImGui::TextDisabled("Disabled"); + ImGui::SameLine(); HelpMarker("The TextDisabled color is stored in ImGuiStyle."); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text/Word Wrapping"); + if (ImGui::TreeNode("Word Wrapping")) + { + // Using shortcut. You can use PushTextWrapPos()/PopTextWrapPos() for more flexibility. + ImGui::TextWrapped( + "This text should automatically wrap on the edge of the window. The current implementation " + "for text wrapping follows simple rules suitable for English and possibly other languages."); + ImGui::Spacing(); + + static float wrap_width = 200.0f; + ImGui::SliderFloat("Wrap width", &wrap_width, -20, 600, "%.0f"); + + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + for (int n = 0; n < 2; n++) + { + ImGui::Text("Test paragraph %d:", n); + ImVec2 pos = ImGui::GetCursorScreenPos(); + ImVec2 marker_min = ImVec2(pos.x + wrap_width, pos.y); + ImVec2 marker_max = ImVec2(pos.x + wrap_width + 10, pos.y + ImGui::GetTextLineHeight()); + ImGui::PushTextWrapPos(ImGui::GetCursorPos().x + wrap_width); + if (n == 0) + ImGui::Text("The lazy dog is a good dog. This paragraph should fit within %.0f pixels. Testing a 1 character word. The quick brown fox jumps over the lazy dog.", wrap_width); + else + ImGui::Text("aaaaaaaa bbbbbbbb, c cccccccc,dddddddd. d eeeeeeee ffffffff. gggggggg!hhhhhhhh"); + + // Draw actual text bounding box, following by marker of our expected limit (should not overlap!) + draw_list->AddRect(ImGui::GetItemRectMin(), ImGui::GetItemRectMax(), IM_COL32(255, 255, 0, 255)); + draw_list->AddRectFilled(marker_min, marker_max, IM_COL32(255, 0, 255, 255)); + ImGui::PopTextWrapPos(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text/UTF-8 Text"); + if (ImGui::TreeNode("UTF-8 Text")) + { + // UTF-8 test with Japanese characters + // (Needs a suitable font? Try "Google Noto" or "Arial Unicode". See docs/FONTS.md for details.) + // - From C++11 you can use the u8"my text" syntax to encode literal strings as UTF-8 + // - For earlier compiler, you may be able to encode your sources as UTF-8 (e.g. in Visual Studio, you + // can save your source files as 'UTF-8 without signature'). + // - FOR THIS DEMO FILE ONLY, BECAUSE WE WANT TO SUPPORT OLD COMPILERS, WE ARE *NOT* INCLUDING RAW UTF-8 + // CHARACTERS IN THIS SOURCE FILE. Instead we are encoding a few strings with hexadecimal constants. + // Don't do this in your application! Please use u8"text in any language" in your application! + // Note that characters values are preserved even by InputText() if the font cannot be displayed, + // so you can safely copy & paste garbled characters into another application. + ImGui::TextWrapped( + "CJK text will only appear if the font was loaded with the appropriate CJK character ranges. " + "Call io.Fonts->AddFontFromFileTTF() manually to load extra character ranges. " + "Read docs/FONTS.md for details."); + ImGui::Text("Hiragana: \xe3\x81\x8b\xe3\x81\x8d\xe3\x81\x8f\xe3\x81\x91\xe3\x81\x93 (kakikukeko)"); // Normally we would use u8"blah blah" with the proper characters directly in the string. + ImGui::Text("Kanjis: \xe6\x97\xa5\xe6\x9c\xac\xe8\xaa\x9e (nihongo)"); + static char buf[32] = "\xe6\x97\xa5\xe6\x9c\xac\xe8\xaa\x9e"; + //static char buf[32] = u8"NIHONGO"; // <- this is how you would write it with C++11, using real kanjis + ImGui::InputText("UTF-8 input", buf, IM_ARRAYSIZE(buf)); + ImGui::TreePop(); + } + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Images"); + if (ImGui::TreeNode("Images")) + { + ImGuiIO& io = ImGui::GetIO(); + ImGui::TextWrapped( + "Below we are displaying the font texture (which is the only texture we have access to in this demo). " + "Use the 'ImTextureID' type as storage to pass pointers or identifier to your own texture data. " + "Hover the texture for a zoomed view!"); + + // Below we are displaying the font texture because it is the only texture we have access to inside the demo! + // Remember that ImTextureID is just storage for whatever you want it to be. It is essentially a value that + // will be passed to the rendering backend via the ImDrawCmd structure. + // If you use one of the default imgui_impl_XXXX.cpp rendering backend, they all have comments at the top + // of their respective source file to specify what they expect to be stored in ImTextureID, for example: + // - The imgui_impl_dx11.cpp renderer expect a 'ID3D11ShaderResourceView*' pointer + // - The imgui_impl_opengl3.cpp renderer expect a GLuint OpenGL texture identifier, etc. + // More: + // - If you decided that ImTextureID = MyEngineTexture*, then you can pass your MyEngineTexture* pointers + // to ImGui::Image(), and gather width/height through your own functions, etc. + // - You can use ShowMetricsWindow() to inspect the draw data that are being passed to your renderer, + // it will help you debug issues if you are confused about it. + // - Consider using the lower-level ImDrawList::AddImage() API, via ImGui::GetWindowDrawList()->AddImage(). + // - Read https://github.com/ocornut/imgui/blob/master/docs/FAQ.md + // - Read https://github.com/ocornut/imgui/wiki/Image-Loading-and-Displaying-Examples + ImTextureID my_tex_id = io.Fonts->TexID; + float my_tex_w = (float)io.Fonts->TexWidth; + float my_tex_h = (float)io.Fonts->TexHeight; + { + static bool use_text_color_for_tint = false; + ImGui::Checkbox("Use Text Color for Tint", &use_text_color_for_tint); + ImGui::Text("%.0fx%.0f", my_tex_w, my_tex_h); + ImVec2 pos = ImGui::GetCursorScreenPos(); + ImVec2 uv_min = ImVec2(0.0f, 0.0f); // Top-left + ImVec2 uv_max = ImVec2(1.0f, 1.0f); // Lower-right + ImVec4 tint_col = use_text_color_for_tint ? ImGui::GetStyleColorVec4(ImGuiCol_Text) : ImVec4(1.0f, 1.0f, 1.0f, 1.0f); // No tint + ImVec4 border_col = ImGui::GetStyleColorVec4(ImGuiCol_Border); + ImGui::Image(my_tex_id, ImVec2(my_tex_w, my_tex_h), uv_min, uv_max, tint_col, border_col); + if (ImGui::BeginItemTooltip()) + { + float region_sz = 32.0f; + float region_x = io.MousePos.x - pos.x - region_sz * 0.5f; + float region_y = io.MousePos.y - pos.y - region_sz * 0.5f; + float zoom = 4.0f; + if (region_x < 0.0f) { region_x = 0.0f; } + else if (region_x > my_tex_w - region_sz) { region_x = my_tex_w - region_sz; } + if (region_y < 0.0f) { region_y = 0.0f; } + else if (region_y > my_tex_h - region_sz) { region_y = my_tex_h - region_sz; } + ImGui::Text("Min: (%.2f, %.2f)", region_x, region_y); + ImGui::Text("Max: (%.2f, %.2f)", region_x + region_sz, region_y + region_sz); + ImVec2 uv0 = ImVec2((region_x) / my_tex_w, (region_y) / my_tex_h); + ImVec2 uv1 = ImVec2((region_x + region_sz) / my_tex_w, (region_y + region_sz) / my_tex_h); + ImGui::Image(my_tex_id, ImVec2(region_sz * zoom, region_sz * zoom), uv0, uv1, tint_col, border_col); + ImGui::EndTooltip(); + } + } + + IMGUI_DEMO_MARKER("Widgets/Images/Textured buttons"); + ImGui::TextWrapped("And now some textured buttons.."); + static int pressed_count = 0; + for (int i = 0; i < 8; i++) + { + // UV coordinates are often (0.0f, 0.0f) and (1.0f, 1.0f) to display an entire textures. + // Here are trying to display only a 32x32 pixels area of the texture, hence the UV computation. + // Read about UV coordinates here: https://github.com/ocornut/imgui/wiki/Image-Loading-and-Displaying-Examples + ImGui::PushID(i); + if (i > 0) + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(i - 1.0f, i - 1.0f)); + ImVec2 size = ImVec2(32.0f, 32.0f); // Size of the image we want to make visible + ImVec2 uv0 = ImVec2(0.0f, 0.0f); // UV coordinates for lower-left + ImVec2 uv1 = ImVec2(32.0f / my_tex_w, 32.0f / my_tex_h); // UV coordinates for (32,32) in our texture + ImVec4 bg_col = ImVec4(0.0f, 0.0f, 0.0f, 1.0f); // Black background + ImVec4 tint_col = ImVec4(1.0f, 1.0f, 1.0f, 1.0f); // No tint + if (ImGui::ImageButton("", my_tex_id, size, uv0, uv1, bg_col, tint_col)) + pressed_count += 1; + if (i > 0) + ImGui::PopStyleVar(); + ImGui::PopID(); + ImGui::SameLine(); + } + ImGui::NewLine(); + ImGui::Text("Pressed %d times.", pressed_count); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Combo"); + if (ImGui::TreeNode("Combo")) + { + // Combo Boxes are also called "Dropdown" in other systems + // Expose flags as checkbox for the demo + static ImGuiComboFlags flags = 0; + ImGui::CheckboxFlags("ImGuiComboFlags_PopupAlignLeft", &flags, ImGuiComboFlags_PopupAlignLeft); + ImGui::SameLine(); HelpMarker("Only makes a difference if the popup is larger than the combo"); + if (ImGui::CheckboxFlags("ImGuiComboFlags_NoArrowButton", &flags, ImGuiComboFlags_NoArrowButton)) + flags &= ~ImGuiComboFlags_NoPreview; // Clear the other flag, as we cannot combine both + if (ImGui::CheckboxFlags("ImGuiComboFlags_NoPreview", &flags, ImGuiComboFlags_NoPreview)) + flags &= ~ImGuiComboFlags_NoArrowButton; // Clear the other flag, as we cannot combine both + + // Using the generic BeginCombo() API, you have full control over how to display the combo contents. + // (your selection data could be an index, a pointer to the object, an id for the object, a flag intrusively + // stored in the object itself, etc.) + const char* items[] = { "AAAA", "BBBB", "CCCC", "DDDD", "EEEE", "FFFF", "GGGG", "HHHH", "IIII", "JJJJ", "KKKK", "LLLLLLL", "MMMM", "OOOOOOO" }; + static int item_current_idx = 0; // Here we store our selection data as an index. + const char* combo_preview_value = items[item_current_idx]; // Pass in the preview value visible before opening the combo (it could be anything) + if (ImGui::BeginCombo("combo 1", combo_preview_value, flags)) + { + for (int n = 0; n < IM_ARRAYSIZE(items); n++) + { + const bool is_selected = (item_current_idx == n); + if (ImGui::Selectable(items[n], is_selected)) + item_current_idx = n; + + // Set the initial focus when opening the combo (scrolling + keyboard navigation focus) + if (is_selected) + ImGui::SetItemDefaultFocus(); + } + ImGui::EndCombo(); + } + + // Simplified one-liner Combo() API, using values packed in a single constant string + // This is a convenience for when the selection set is small and known at compile-time. + static int item_current_2 = 0; + ImGui::Combo("combo 2 (one-liner)", &item_current_2, "aaaa\0bbbb\0cccc\0dddd\0eeee\0\0"); + + // Simplified one-liner Combo() using an array of const char* + // This is not very useful (may obsolete): prefer using BeginCombo()/EndCombo() for full control. + static int item_current_3 = -1; // If the selection isn't within 0..count, Combo won't display a preview + ImGui::Combo("combo 3 (array)", &item_current_3, items, IM_ARRAYSIZE(items)); + + // Simplified one-liner Combo() using an accessor function + static int item_current_4 = 0; + ImGui::Combo("combo 4 (function)", &item_current_4, [](void* data, int n) { return ((const char**)data)[n]; }, items, IM_ARRAYSIZE(items)); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/List Boxes"); + if (ImGui::TreeNode("List boxes")) + { + // Using the generic BeginListBox() API, you have full control over how to display the combo contents. + // (your selection data could be an index, a pointer to the object, an id for the object, a flag intrusively + // stored in the object itself, etc.) + const char* items[] = { "AAAA", "BBBB", "CCCC", "DDDD", "EEEE", "FFFF", "GGGG", "HHHH", "IIII", "JJJJ", "KKKK", "LLLLLLL", "MMMM", "OOOOOOO" }; + static int item_current_idx = 0; // Here we store our selection data as an index. + if (ImGui::BeginListBox("listbox 1")) + { + for (int n = 0; n < IM_ARRAYSIZE(items); n++) + { + const bool is_selected = (item_current_idx == n); + if (ImGui::Selectable(items[n], is_selected)) + item_current_idx = n; + + // Set the initial focus when opening the combo (scrolling + keyboard navigation focus) + if (is_selected) + ImGui::SetItemDefaultFocus(); + } + ImGui::EndListBox(); + } + + // Custom size: use all width, 5 items tall + ImGui::Text("Full-width:"); + if (ImGui::BeginListBox("##listbox 2", ImVec2(-FLT_MIN, 5 * ImGui::GetTextLineHeightWithSpacing()))) + { + for (int n = 0; n < IM_ARRAYSIZE(items); n++) + { + const bool is_selected = (item_current_idx == n); + if (ImGui::Selectable(items[n], is_selected)) + item_current_idx = n; + + // Set the initial focus when opening the combo (scrolling + keyboard navigation focus) + if (is_selected) + ImGui::SetItemDefaultFocus(); + } + ImGui::EndListBox(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Selectables"); + if (ImGui::TreeNode("Selectables")) + { + // Selectable() has 2 overloads: + // - The one taking "bool selected" as a read-only selection information. + // When Selectable() has been clicked it returns true and you can alter selection state accordingly. + // - The one taking "bool* p_selected" as a read-write selection information (convenient in some cases) + // The earlier is more flexible, as in real application your selection may be stored in many different ways + // and not necessarily inside a bool value (e.g. in flags within objects, as an external list, etc). + IMGUI_DEMO_MARKER("Widgets/Selectables/Basic"); + if (ImGui::TreeNode("Basic")) + { + static bool selection[5] = { false, true, false, false }; + ImGui::Selectable("1. I am selectable", &selection[0]); + ImGui::Selectable("2. I am selectable", &selection[1]); + ImGui::Selectable("3. I am selectable", &selection[2]); + if (ImGui::Selectable("4. I am double clickable", selection[3], ImGuiSelectableFlags_AllowDoubleClick)) + if (ImGui::IsMouseDoubleClicked(0)) + selection[3] = !selection[3]; + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Selectables/Single Selection"); + if (ImGui::TreeNode("Selection State: Single Selection")) + { + static int selected = -1; + for (int n = 0; n < 5; n++) + { + char buf[32]; + sprintf(buf, "Object %d", n); + if (ImGui::Selectable(buf, selected == n)) + selected = n; + } + ImGui::TreePop(); + } + IMGUI_DEMO_MARKER("Widgets/Selectables/Multiple Selection"); + if (ImGui::TreeNode("Selection State: Multiple Selection")) + { + HelpMarker("Hold CTRL and click to select multiple items."); + static bool selection[5] = { false, false, false, false, false }; + for (int n = 0; n < 5; n++) + { + char buf[32]; + sprintf(buf, "Object %d", n); + if (ImGui::Selectable(buf, selection[n])) + { + if (!ImGui::GetIO().KeyCtrl) // Clear selection when CTRL is not held + memset(selection, 0, sizeof(selection)); + selection[n] ^= 1; + } + } + ImGui::TreePop(); + } + IMGUI_DEMO_MARKER("Widgets/Selectables/Rendering more items on the same line"); + if (ImGui::TreeNode("Rendering more items on the same line")) + { + // (1) Using SetNextItemAllowOverlap() + // (2) Using the Selectable() override that takes "bool* p_selected" parameter, the bool value is toggled automatically. + static bool selected[3] = { false, false, false }; + ImGui::SetNextItemAllowOverlap(); ImGui::Selectable("main.c", &selected[0]); ImGui::SameLine(); ImGui::SmallButton("Link 1"); + ImGui::SetNextItemAllowOverlap(); ImGui::Selectable("Hello.cpp", &selected[1]); ImGui::SameLine(); ImGui::SmallButton("Link 2"); + ImGui::SetNextItemAllowOverlap(); ImGui::Selectable("Hello.h", &selected[2]); ImGui::SameLine(); ImGui::SmallButton("Link 3"); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Selectables/In columns"); + if (ImGui::TreeNode("In columns")) + { + static bool selected[10] = {}; + + if (ImGui::BeginTable("split1", 3, ImGuiTableFlags_Resizable | ImGuiTableFlags_NoSavedSettings | ImGuiTableFlags_Borders)) + { + for (int i = 0; i < 10; i++) + { + char label[32]; + sprintf(label, "Item %d", i); + ImGui::TableNextColumn(); + ImGui::Selectable(label, &selected[i]); // FIXME-TABLE: Selection overlap + } + ImGui::EndTable(); + } + ImGui::Spacing(); + if (ImGui::BeginTable("split2", 3, ImGuiTableFlags_Resizable | ImGuiTableFlags_NoSavedSettings | ImGuiTableFlags_Borders)) + { + for (int i = 0; i < 10; i++) + { + char label[32]; + sprintf(label, "Item %d", i); + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + ImGui::Selectable(label, &selected[i], ImGuiSelectableFlags_SpanAllColumns); + ImGui::TableNextColumn(); + ImGui::Text("Some other contents"); + ImGui::TableNextColumn(); + ImGui::Text("123456"); + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Selectables/Grid"); + if (ImGui::TreeNode("Grid")) + { + static char selected[4][4] = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } }; + + // Add in a bit of silly fun... + const float time = (float)ImGui::GetTime(); + const bool winning_state = memchr(selected, 0, sizeof(selected)) == NULL; // If all cells are selected... + if (winning_state) + ImGui::PushStyleVar(ImGuiStyleVar_SelectableTextAlign, ImVec2(0.5f + 0.5f * cosf(time * 2.0f), 0.5f + 0.5f * sinf(time * 3.0f))); + + for (int y = 0; y < 4; y++) + for (int x = 0; x < 4; x++) + { + if (x > 0) + ImGui::SameLine(); + ImGui::PushID(y * 4 + x); + if (ImGui::Selectable("Sailor", selected[y][x] != 0, 0, ImVec2(50, 50))) + { + // Toggle clicked cell + toggle neighbors + selected[y][x] ^= 1; + if (x > 0) { selected[y][x - 1] ^= 1; } + if (x < 3) { selected[y][x + 1] ^= 1; } + if (y > 0) { selected[y - 1][x] ^= 1; } + if (y < 3) { selected[y + 1][x] ^= 1; } + } + ImGui::PopID(); + } + + if (winning_state) + ImGui::PopStyleVar(); + ImGui::TreePop(); + } + IMGUI_DEMO_MARKER("Widgets/Selectables/Alignment"); + if (ImGui::TreeNode("Alignment")) + { + HelpMarker( + "By default, Selectables uses style.SelectableTextAlign but it can be overridden on a per-item " + "basis using PushStyleVar(). You'll probably want to always keep your default situation to " + "left-align otherwise it becomes difficult to layout multiple items on a same line"); + static bool selected[3 * 3] = { true, false, true, false, true, false, true, false, true }; + for (int y = 0; y < 3; y++) + { + for (int x = 0; x < 3; x++) + { + ImVec2 alignment = ImVec2((float)x / 2.0f, (float)y / 2.0f); + char name[32]; + sprintf(name, "(%.1f,%.1f)", alignment.x, alignment.y); + if (x > 0) ImGui::SameLine(); + ImGui::PushStyleVar(ImGuiStyleVar_SelectableTextAlign, alignment); + ImGui::Selectable(name, &selected[3 * y + x], ImGuiSelectableFlags_None, ImVec2(80, 80)); + ImGui::PopStyleVar(); + } + } + ImGui::TreePop(); + } + ImGui::TreePop(); + } + + // To wire InputText() with std::string or any other custom string type, + // see the "Text Input > Resize Callback" section of this demo, and the misc/cpp/imgui_stdlib.h file. + IMGUI_DEMO_MARKER("Widgets/Text Input"); + if (ImGui::TreeNode("Text Input")) + { + IMGUI_DEMO_MARKER("Widgets/Text Input/Multi-line Text Input"); + if (ImGui::TreeNode("Multi-line Text Input")) + { + // Note: we are using a fixed-sized buffer for simplicity here. See ImGuiInputTextFlags_CallbackResize + // and the code in misc/cpp/imgui_stdlib.h for how to setup InputText() for dynamically resizing strings. + static char text[1024 * 16] = + "/*\n" + " The Pentium F00F bug, shorthand for F0 0F C7 C8,\n" + " the hexadecimal encoding of one offending instruction,\n" + " more formally, the invalid operand with locked CMPXCHG8B\n" + " instruction bug, is a design flaw in the majority of\n" + " Intel Pentium, Pentium MMX, and Pentium OverDrive\n" + " processors (all in the P5 microarchitecture).\n" + "*/\n\n" + "label:\n" + "\tlock cmpxchg8b eax\n"; + + static ImGuiInputTextFlags flags = ImGuiInputTextFlags_AllowTabInput; + HelpMarker("You can use the ImGuiInputTextFlags_CallbackResize facility if you need to wire InputTextMultiline() to a dynamic string type. See misc/cpp/imgui_stdlib.h for an example. (This is not demonstrated in imgui_demo.cpp because we don't want to include in here)"); + ImGui::CheckboxFlags("ImGuiInputTextFlags_ReadOnly", &flags, ImGuiInputTextFlags_ReadOnly); + ImGui::CheckboxFlags("ImGuiInputTextFlags_AllowTabInput", &flags, ImGuiInputTextFlags_AllowTabInput); + ImGui::CheckboxFlags("ImGuiInputTextFlags_CtrlEnterForNewLine", &flags, ImGuiInputTextFlags_CtrlEnterForNewLine); + ImGui::InputTextMultiline("##source", text, IM_ARRAYSIZE(text), ImVec2(-FLT_MIN, ImGui::GetTextLineHeight() * 16), flags); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text Input/Filtered Text Input"); + if (ImGui::TreeNode("Filtered Text Input")) + { + struct TextFilters + { + // Modify character input by altering 'data->Eventchar' (ImGuiInputTextFlags_CallbackCharFilter callback) + static int FilterCasingSwap(ImGuiInputTextCallbackData* data) + { + if (data->EventChar >= 'a' && data->EventChar <= 'z') { data->EventChar -= 'a' - 'A'; } // Lowercase becomes uppercase + else if (data->EventChar >= 'A' && data->EventChar <= 'Z') { data->EventChar += 'a' - 'A'; } // Uppercase becomes lowercase + return 0; + } + + // Return 0 (pass) if the character is 'i' or 'm' or 'g' or 'u' or 'i', otherwise return 1 (filter out) + static int FilterImGuiLetters(ImGuiInputTextCallbackData* data) + { + if (data->EventChar < 256 && strchr("imgui", (char)data->EventChar)) + return 0; + return 1; + } + }; + + static char buf1[32] = ""; ImGui::InputText("default", buf1, 32); + static char buf2[32] = ""; ImGui::InputText("decimal", buf2, 32, ImGuiInputTextFlags_CharsDecimal); + static char buf3[32] = ""; ImGui::InputText("hexadecimal", buf3, 32, ImGuiInputTextFlags_CharsHexadecimal | ImGuiInputTextFlags_CharsUppercase); + static char buf4[32] = ""; ImGui::InputText("uppercase", buf4, 32, ImGuiInputTextFlags_CharsUppercase); + static char buf5[32] = ""; ImGui::InputText("no blank", buf5, 32, ImGuiInputTextFlags_CharsNoBlank); + static char buf6[32] = ""; ImGui::InputText("casing swap", buf6, 32, ImGuiInputTextFlags_CallbackCharFilter, TextFilters::FilterCasingSwap); // Use CharFilter callback to replace characters. + static char buf7[32] = ""; ImGui::InputText("\"imgui\"", buf7, 32, ImGuiInputTextFlags_CallbackCharFilter, TextFilters::FilterImGuiLetters); // Use CharFilter callback to disable some characters. + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text Input/Password input"); + if (ImGui::TreeNode("Password Input")) + { + static char password[64] = "password123"; + ImGui::InputText("password", password, IM_ARRAYSIZE(password), ImGuiInputTextFlags_Password); + ImGui::SameLine(); HelpMarker("Display all characters as '*'.\nDisable clipboard cut and copy.\nDisable logging.\n"); + ImGui::InputTextWithHint("password (w/ hint)", "", password, IM_ARRAYSIZE(password), ImGuiInputTextFlags_Password); + ImGui::InputText("password (clear)", password, IM_ARRAYSIZE(password)); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text Input/Completion, History, Edit Callbacks"); + if (ImGui::TreeNode("Completion, History, Edit Callbacks")) + { + struct Funcs + { + static int MyCallback(ImGuiInputTextCallbackData* data) + { + if (data->EventFlag == ImGuiInputTextFlags_CallbackCompletion) + { + data->InsertChars(data->CursorPos, ".."); + } + else if (data->EventFlag == ImGuiInputTextFlags_CallbackHistory) + { + if (data->EventKey == ImGuiKey_UpArrow) + { + data->DeleteChars(0, data->BufTextLen); + data->InsertChars(0, "Pressed Up!"); + data->SelectAll(); + } + else if (data->EventKey == ImGuiKey_DownArrow) + { + data->DeleteChars(0, data->BufTextLen); + data->InsertChars(0, "Pressed Down!"); + data->SelectAll(); + } + } + else if (data->EventFlag == ImGuiInputTextFlags_CallbackEdit) + { + // Toggle casing of first character + char c = data->Buf[0]; + if ((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z')) data->Buf[0] ^= 32; + data->BufDirty = true; + + // Increment a counter + int* p_int = (int*)data->UserData; + *p_int = *p_int + 1; + } + return 0; + } + }; + static char buf1[64]; + ImGui::InputText("Completion", buf1, 64, ImGuiInputTextFlags_CallbackCompletion, Funcs::MyCallback); + ImGui::SameLine(); HelpMarker("Here we append \"..\" each time Tab is pressed. See 'Examples>Console' for a more meaningful demonstration of using this callback."); + + static char buf2[64]; + ImGui::InputText("History", buf2, 64, ImGuiInputTextFlags_CallbackHistory, Funcs::MyCallback); + ImGui::SameLine(); HelpMarker("Here we replace and select text each time Up/Down are pressed. See 'Examples>Console' for a more meaningful demonstration of using this callback."); + + static char buf3[64]; + static int edit_count = 0; + ImGui::InputText("Edit", buf3, 64, ImGuiInputTextFlags_CallbackEdit, Funcs::MyCallback, (void*)&edit_count); + ImGui::SameLine(); HelpMarker("Here we toggle the casing of the first character on every edit + count edits."); + ImGui::SameLine(); ImGui::Text("(%d)", edit_count); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text Input/Resize Callback"); + if (ImGui::TreeNode("Resize Callback")) + { + // To wire InputText() with std::string or any other custom string type, + // you can use the ImGuiInputTextFlags_CallbackResize flag + create a custom ImGui::InputText() wrapper + // using your preferred type. See misc/cpp/imgui_stdlib.h for an implementation of this using std::string. + HelpMarker( + "Using ImGuiInputTextFlags_CallbackResize to wire your custom string type to InputText().\n\n" + "See misc/cpp/imgui_stdlib.h for an implementation of this for std::string."); + struct Funcs + { + static int MyResizeCallback(ImGuiInputTextCallbackData* data) + { + if (data->EventFlag == ImGuiInputTextFlags_CallbackResize) + { + ImVector* my_str = (ImVector*)data->UserData; + IM_ASSERT(my_str->begin() == data->Buf); + my_str->resize(data->BufSize); // NB: On resizing calls, generally data->BufSize == data->BufTextLen + 1 + data->Buf = my_str->begin(); + } + return 0; + } + + // Note: Because ImGui:: is a namespace you would typically add your own function into the namespace. + // For example, you code may declare a function 'ImGui::InputText(const char* label, MyString* my_str)' + static bool MyInputTextMultiline(const char* label, ImVector* my_str, const ImVec2& size = ImVec2(0, 0), ImGuiInputTextFlags flags = 0) + { + IM_ASSERT((flags & ImGuiInputTextFlags_CallbackResize) == 0); + return ImGui::InputTextMultiline(label, my_str->begin(), (size_t)my_str->size(), size, flags | ImGuiInputTextFlags_CallbackResize, Funcs::MyResizeCallback, (void*)my_str); + } + }; + + // For this demo we are using ImVector as a string container. + // Note that because we need to store a terminating zero character, our size/capacity are 1 more + // than usually reported by a typical string class. + static ImVector my_str; + if (my_str.empty()) + my_str.push_back(0); + Funcs::MyInputTextMultiline("##MyStr", &my_str, ImVec2(-FLT_MIN, ImGui::GetTextLineHeight() * 16)); + ImGui::Text("Data: %p\nSize: %d\nCapacity: %d", (void*)my_str.begin(), my_str.size(), my_str.capacity()); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text Input/Miscellaneous"); + if (ImGui::TreeNode("Miscellaneous")) + { + static char buf1[16]; + static ImGuiInputTextFlags flags = ImGuiInputTextFlags_EscapeClearsAll; + ImGui::CheckboxFlags("ImGuiInputTextFlags_EscapeClearsAll", &flags, ImGuiInputTextFlags_EscapeClearsAll); + ImGui::CheckboxFlags("ImGuiInputTextFlags_ReadOnly", &flags, ImGuiInputTextFlags_ReadOnly); + ImGui::CheckboxFlags("ImGuiInputTextFlags_NoUndoRedo", &flags, ImGuiInputTextFlags_NoUndoRedo); + ImGui::InputText("Hello", buf1, IM_ARRAYSIZE(buf1), flags); + ImGui::TreePop(); + } + + ImGui::TreePop(); + } + + // Tabs + IMGUI_DEMO_MARKER("Widgets/Tabs"); + if (ImGui::TreeNode("Tabs")) + { + IMGUI_DEMO_MARKER("Widgets/Tabs/Basic"); + if (ImGui::TreeNode("Basic")) + { + ImGuiTabBarFlags tab_bar_flags = ImGuiTabBarFlags_None; + if (ImGui::BeginTabBar("MyTabBar", tab_bar_flags)) + { + if (ImGui::BeginTabItem("Avocado")) + { + ImGui::Text("This is the Avocado tab!\nblah blah blah blah blah"); + ImGui::EndTabItem(); + } + if (ImGui::BeginTabItem("Broccoli")) + { + ImGui::Text("This is the Broccoli tab!\nblah blah blah blah blah"); + ImGui::EndTabItem(); + } + if (ImGui::BeginTabItem("Cucumber")) + { + ImGui::Text("This is the Cucumber tab!\nblah blah blah blah blah"); + ImGui::EndTabItem(); + } + ImGui::EndTabBar(); + } + ImGui::Separator(); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Tabs/Advanced & Close Button"); + if (ImGui::TreeNode("Advanced & Close Button")) + { + // Expose a couple of the available flags. In most cases you may just call BeginTabBar() with no flags (0). + static ImGuiTabBarFlags tab_bar_flags = ImGuiTabBarFlags_Reorderable; + ImGui::CheckboxFlags("ImGuiTabBarFlags_Reorderable", &tab_bar_flags, ImGuiTabBarFlags_Reorderable); + ImGui::CheckboxFlags("ImGuiTabBarFlags_AutoSelectNewTabs", &tab_bar_flags, ImGuiTabBarFlags_AutoSelectNewTabs); + ImGui::CheckboxFlags("ImGuiTabBarFlags_TabListPopupButton", &tab_bar_flags, ImGuiTabBarFlags_TabListPopupButton); + ImGui::CheckboxFlags("ImGuiTabBarFlags_NoCloseWithMiddleMouseButton", &tab_bar_flags, ImGuiTabBarFlags_NoCloseWithMiddleMouseButton); + if ((tab_bar_flags & ImGuiTabBarFlags_FittingPolicyMask_) == 0) + tab_bar_flags |= ImGuiTabBarFlags_FittingPolicyDefault_; + if (ImGui::CheckboxFlags("ImGuiTabBarFlags_FittingPolicyResizeDown", &tab_bar_flags, ImGuiTabBarFlags_FittingPolicyResizeDown)) + tab_bar_flags &= ~(ImGuiTabBarFlags_FittingPolicyMask_ ^ ImGuiTabBarFlags_FittingPolicyResizeDown); + if (ImGui::CheckboxFlags("ImGuiTabBarFlags_FittingPolicyScroll", &tab_bar_flags, ImGuiTabBarFlags_FittingPolicyScroll)) + tab_bar_flags &= ~(ImGuiTabBarFlags_FittingPolicyMask_ ^ ImGuiTabBarFlags_FittingPolicyScroll); + + // Tab Bar + const char* names[4] = { "Artichoke", "Beetroot", "Celery", "Daikon" }; + static bool opened[4] = { true, true, true, true }; // Persistent user state + for (int n = 0; n < IM_ARRAYSIZE(opened); n++) + { + if (n > 0) { ImGui::SameLine(); } + ImGui::Checkbox(names[n], &opened[n]); + } + + // Passing a bool* to BeginTabItem() is similar to passing one to Begin(): + // the underlying bool will be set to false when the tab is closed. + if (ImGui::BeginTabBar("MyTabBar", tab_bar_flags)) + { + for (int n = 0; n < IM_ARRAYSIZE(opened); n++) + if (opened[n] && ImGui::BeginTabItem(names[n], &opened[n], ImGuiTabItemFlags_None)) + { + ImGui::Text("This is the %s tab!", names[n]); + if (n & 1) + ImGui::Text("I am an odd tab."); + ImGui::EndTabItem(); + } + ImGui::EndTabBar(); + } + ImGui::Separator(); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Tabs/TabItemButton & Leading-Trailing flags"); + if (ImGui::TreeNode("TabItemButton & Leading/Trailing flags")) + { + static ImVector active_tabs; + static int next_tab_id = 0; + if (next_tab_id == 0) // Initialize with some default tabs + for (int i = 0; i < 3; i++) + active_tabs.push_back(next_tab_id++); + + // TabItemButton() and Leading/Trailing flags are distinct features which we will demo together. + // (It is possible to submit regular tabs with Leading/Trailing flags, or TabItemButton tabs without Leading/Trailing flags... + // but they tend to make more sense together) + static bool show_leading_button = true; + static bool show_trailing_button = true; + ImGui::Checkbox("Show Leading TabItemButton()", &show_leading_button); + ImGui::Checkbox("Show Trailing TabItemButton()", &show_trailing_button); + + // Expose some other flags which are useful to showcase how they interact with Leading/Trailing tabs + static ImGuiTabBarFlags tab_bar_flags = ImGuiTabBarFlags_AutoSelectNewTabs | ImGuiTabBarFlags_Reorderable | ImGuiTabBarFlags_FittingPolicyResizeDown; + ImGui::CheckboxFlags("ImGuiTabBarFlags_TabListPopupButton", &tab_bar_flags, ImGuiTabBarFlags_TabListPopupButton); + if (ImGui::CheckboxFlags("ImGuiTabBarFlags_FittingPolicyResizeDown", &tab_bar_flags, ImGuiTabBarFlags_FittingPolicyResizeDown)) + tab_bar_flags &= ~(ImGuiTabBarFlags_FittingPolicyMask_ ^ ImGuiTabBarFlags_FittingPolicyResizeDown); + if (ImGui::CheckboxFlags("ImGuiTabBarFlags_FittingPolicyScroll", &tab_bar_flags, ImGuiTabBarFlags_FittingPolicyScroll)) + tab_bar_flags &= ~(ImGuiTabBarFlags_FittingPolicyMask_ ^ ImGuiTabBarFlags_FittingPolicyScroll); + + if (ImGui::BeginTabBar("MyTabBar", tab_bar_flags)) + { + // Demo a Leading TabItemButton(): click the "?" button to open a menu + if (show_leading_button) + if (ImGui::TabItemButton("?", ImGuiTabItemFlags_Leading | ImGuiTabItemFlags_NoTooltip)) + ImGui::OpenPopup("MyHelpMenu"); + if (ImGui::BeginPopup("MyHelpMenu")) + { + ImGui::Selectable("Hello!"); + ImGui::EndPopup(); + } + + // Demo Trailing Tabs: click the "+" button to add a new tab (in your app you may want to use a font icon instead of the "+") + // Note that we submit it before the regular tabs, but because of the ImGuiTabItemFlags_Trailing flag it will always appear at the end. + if (show_trailing_button) + if (ImGui::TabItemButton("+", ImGuiTabItemFlags_Trailing | ImGuiTabItemFlags_NoTooltip)) + active_tabs.push_back(next_tab_id++); // Add new tab + + // Submit our regular tabs + for (int n = 0; n < active_tabs.Size; ) + { + bool open = true; + char name[16]; + snprintf(name, IM_ARRAYSIZE(name), "%04d", active_tabs[n]); + if (ImGui::BeginTabItem(name, &open, ImGuiTabItemFlags_None)) + { + ImGui::Text("This is the %s tab!", name); + ImGui::EndTabItem(); + } + + if (!open) + active_tabs.erase(active_tabs.Data + n); + else + n++; + } + + ImGui::EndTabBar(); + } + ImGui::Separator(); + ImGui::TreePop(); + } + ImGui::TreePop(); + } + + // Plot/Graph widgets are not very good. + // Consider using a third-party library such as ImPlot: https://github.com/epezent/implot + // (see others https://github.com/ocornut/imgui/wiki/Useful-Extensions) + IMGUI_DEMO_MARKER("Widgets/Plotting"); + if (ImGui::TreeNode("Plotting")) + { + static bool animate = true; + ImGui::Checkbox("Animate", &animate); + + // Plot as lines and plot as histogram + IMGUI_DEMO_MARKER("Widgets/Plotting/PlotLines, PlotHistogram"); + static float arr[] = { 0.6f, 0.1f, 1.0f, 0.5f, 0.92f, 0.1f, 0.2f }; + ImGui::PlotLines("Frame Times", arr, IM_ARRAYSIZE(arr)); + ImGui::PlotHistogram("Histogram", arr, IM_ARRAYSIZE(arr), 0, NULL, 0.0f, 1.0f, ImVec2(0, 80.0f)); + + // Fill an array of contiguous float values to plot + // Tip: If your float aren't contiguous but part of a structure, you can pass a pointer to your first float + // and the sizeof() of your structure in the "stride" parameter. + static float values[90] = {}; + static int values_offset = 0; + static double refresh_time = 0.0; + if (!animate || refresh_time == 0.0) + refresh_time = ImGui::GetTime(); + while (refresh_time < ImGui::GetTime()) // Create data at fixed 60 Hz rate for the demo + { + static float phase = 0.0f; + values[values_offset] = cosf(phase); + values_offset = (values_offset + 1) % IM_ARRAYSIZE(values); + phase += 0.10f * values_offset; + refresh_time += 1.0f / 60.0f; + } + + // Plots can display overlay texts + // (in this example, we will display an average value) + { + float average = 0.0f; + for (int n = 0; n < IM_ARRAYSIZE(values); n++) + average += values[n]; + average /= (float)IM_ARRAYSIZE(values); + char overlay[32]; + sprintf(overlay, "avg %f", average); + ImGui::PlotLines("Lines", values, IM_ARRAYSIZE(values), values_offset, overlay, -1.0f, 1.0f, ImVec2(0, 80.0f)); + } + + // Use functions to generate output + // FIXME: This is actually VERY awkward because current plot API only pass in indices. + // We probably want an API passing floats and user provide sample rate/count. + struct Funcs + { + static float Sin(void*, int i) { return sinf(i * 0.1f); } + static float Saw(void*, int i) { return (i & 1) ? 1.0f : -1.0f; } + }; + static int func_type = 0, display_count = 70; + ImGui::SeparatorText("Functions"); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8); + ImGui::Combo("func", &func_type, "Sin\0Saw\0"); + ImGui::SameLine(); + ImGui::SliderInt("Sample count", &display_count, 1, 400); + float (*func)(void*, int) = (func_type == 0) ? Funcs::Sin : Funcs::Saw; + ImGui::PlotLines("Lines", func, NULL, display_count, 0, NULL, -1.0f, 1.0f, ImVec2(0, 80)); + ImGui::PlotHistogram("Histogram", func, NULL, display_count, 0, NULL, -1.0f, 1.0f, ImVec2(0, 80)); + ImGui::Separator(); + + // Animate a simple progress bar + IMGUI_DEMO_MARKER("Widgets/Plotting/ProgressBar"); + static float progress = 0.0f, progress_dir = 1.0f; + if (animate) + { + progress += progress_dir * 0.4f * ImGui::GetIO().DeltaTime; + if (progress >= +1.1f) { progress = +1.1f; progress_dir *= -1.0f; } + if (progress <= -0.1f) { progress = -0.1f; progress_dir *= -1.0f; } + } + + // Typically we would use ImVec2(-1.0f,0.0f) or ImVec2(-FLT_MIN,0.0f) to use all available width, + // or ImVec2(width,0.0f) for a specified width. ImVec2(0.0f,0.0f) uses ItemWidth. + ImGui::ProgressBar(progress, ImVec2(0.0f, 0.0f)); + ImGui::SameLine(0.0f, ImGui::GetStyle().ItemInnerSpacing.x); + ImGui::Text("Progress Bar"); + + float progress_saturated = IM_CLAMP(progress, 0.0f, 1.0f); + char buf[32]; + sprintf(buf, "%d/%d", (int)(progress_saturated * 1753), 1753); + ImGui::ProgressBar(progress, ImVec2(0.f, 0.f), buf); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Color"); + if (ImGui::TreeNode("Color/Picker Widgets")) + { + static ImVec4 color = ImVec4(114.0f / 255.0f, 144.0f / 255.0f, 154.0f / 255.0f, 200.0f / 255.0f); + + static bool alpha_preview = true; + static bool alpha_half_preview = false; + static bool drag_and_drop = true; + static bool options_menu = true; + static bool hdr = false; + ImGui::SeparatorText("Options"); + ImGui::Checkbox("With Alpha Preview", &alpha_preview); + ImGui::Checkbox("With Half Alpha Preview", &alpha_half_preview); + ImGui::Checkbox("With Drag and Drop", &drag_and_drop); + ImGui::Checkbox("With Options Menu", &options_menu); ImGui::SameLine(); HelpMarker("Right-click on the individual color widget to show options."); + ImGui::Checkbox("With HDR", &hdr); ImGui::SameLine(); HelpMarker("Currently all this does is to lift the 0..1 limits on dragging widgets."); + ImGuiColorEditFlags misc_flags = (hdr ? ImGuiColorEditFlags_HDR : 0) | (drag_and_drop ? 0 : ImGuiColorEditFlags_NoDragDrop) | (alpha_half_preview ? ImGuiColorEditFlags_AlphaPreviewHalf : (alpha_preview ? ImGuiColorEditFlags_AlphaPreview : 0)) | (options_menu ? 0 : ImGuiColorEditFlags_NoOptions); + + IMGUI_DEMO_MARKER("Widgets/Color/ColorEdit"); + ImGui::SeparatorText("Inline color editor"); + ImGui::Text("Color widget:"); + ImGui::SameLine(); HelpMarker( + "Click on the color square to open a color picker.\n" + "CTRL+click on individual component to input value.\n"); + ImGui::ColorEdit3("MyColor##1", (float*)&color, misc_flags); + + IMGUI_DEMO_MARKER("Widgets/Color/ColorEdit (HSV, with Alpha)"); + ImGui::Text("Color widget HSV with Alpha:"); + ImGui::ColorEdit4("MyColor##2", (float*)&color, ImGuiColorEditFlags_DisplayHSV | misc_flags); + + IMGUI_DEMO_MARKER("Widgets/Color/ColorEdit (float display)"); + ImGui::Text("Color widget with Float Display:"); + ImGui::ColorEdit4("MyColor##2f", (float*)&color, ImGuiColorEditFlags_Float | misc_flags); + + IMGUI_DEMO_MARKER("Widgets/Color/ColorButton (with Picker)"); + ImGui::Text("Color button with Picker:"); + ImGui::SameLine(); HelpMarker( + "With the ImGuiColorEditFlags_NoInputs flag you can hide all the slider/text inputs.\n" + "With the ImGuiColorEditFlags_NoLabel flag you can pass a non-empty label which will only " + "be used for the tooltip and picker popup."); + ImGui::ColorEdit4("MyColor##3", (float*)&color, ImGuiColorEditFlags_NoInputs | ImGuiColorEditFlags_NoLabel | misc_flags); + + IMGUI_DEMO_MARKER("Widgets/Color/ColorButton (with custom Picker popup)"); + ImGui::Text("Color button with Custom Picker Popup:"); + + // Generate a default palette. The palette will persist and can be edited. + static bool saved_palette_init = true; + static ImVec4 saved_palette[32] = {}; + if (saved_palette_init) + { + for (int n = 0; n < IM_ARRAYSIZE(saved_palette); n++) + { + ImGui::ColorConvertHSVtoRGB(n / 31.0f, 0.8f, 0.8f, + saved_palette[n].x, saved_palette[n].y, saved_palette[n].z); + saved_palette[n].w = 1.0f; // Alpha + } + saved_palette_init = false; + } + + static ImVec4 backup_color; + bool open_popup = ImGui::ColorButton("MyColor##3b", color, misc_flags); + ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x); + open_popup |= ImGui::Button("Palette"); + if (open_popup) + { + ImGui::OpenPopup("mypicker"); + backup_color = color; + } + if (ImGui::BeginPopup("mypicker")) + { + ImGui::Text("MY CUSTOM COLOR PICKER WITH AN AMAZING PALETTE!"); + ImGui::Separator(); + ImGui::ColorPicker4("##picker", (float*)&color, misc_flags | ImGuiColorEditFlags_NoSidePreview | ImGuiColorEditFlags_NoSmallPreview); + ImGui::SameLine(); + + ImGui::BeginGroup(); // Lock X position + ImGui::Text("Current"); + ImGui::ColorButton("##current", color, ImGuiColorEditFlags_NoPicker | ImGuiColorEditFlags_AlphaPreviewHalf, ImVec2(60, 40)); + ImGui::Text("Previous"); + if (ImGui::ColorButton("##previous", backup_color, ImGuiColorEditFlags_NoPicker | ImGuiColorEditFlags_AlphaPreviewHalf, ImVec2(60, 40))) + color = backup_color; + ImGui::Separator(); + ImGui::Text("Palette"); + for (int n = 0; n < IM_ARRAYSIZE(saved_palette); n++) + { + ImGui::PushID(n); + if ((n % 8) != 0) + ImGui::SameLine(0.0f, ImGui::GetStyle().ItemSpacing.y); + + ImGuiColorEditFlags palette_button_flags = ImGuiColorEditFlags_NoAlpha | ImGuiColorEditFlags_NoPicker | ImGuiColorEditFlags_NoTooltip; + if (ImGui::ColorButton("##palette", saved_palette[n], palette_button_flags, ImVec2(20, 20))) + color = ImVec4(saved_palette[n].x, saved_palette[n].y, saved_palette[n].z, color.w); // Preserve alpha! + + // Allow user to drop colors into each palette entry. Note that ColorButton() is already a + // drag source by default, unless specifying the ImGuiColorEditFlags_NoDragDrop flag. + if (ImGui::BeginDragDropTarget()) + { + if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload(IMGUI_PAYLOAD_TYPE_COLOR_3F)) + memcpy((float*)&saved_palette[n], payload->Data, sizeof(float) * 3); + if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload(IMGUI_PAYLOAD_TYPE_COLOR_4F)) + memcpy((float*)&saved_palette[n], payload->Data, sizeof(float) * 4); + ImGui::EndDragDropTarget(); + } + + ImGui::PopID(); + } + ImGui::EndGroup(); + ImGui::EndPopup(); + } + + IMGUI_DEMO_MARKER("Widgets/Color/ColorButton (simple)"); + ImGui::Text("Color button only:"); + static bool no_border = false; + ImGui::Checkbox("ImGuiColorEditFlags_NoBorder", &no_border); + ImGui::ColorButton("MyColor##3c", *(ImVec4*)&color, misc_flags | (no_border ? ImGuiColorEditFlags_NoBorder : 0), ImVec2(80, 80)); + + IMGUI_DEMO_MARKER("Widgets/Color/ColorPicker"); + ImGui::SeparatorText("Color picker"); + static bool alpha = true; + static bool alpha_bar = true; + static bool side_preview = true; + static bool ref_color = false; + static ImVec4 ref_color_v(1.0f, 0.0f, 1.0f, 0.5f); + static int display_mode = 0; + static int picker_mode = 0; + ImGui::Checkbox("With Alpha", &alpha); + ImGui::Checkbox("With Alpha Bar", &alpha_bar); + ImGui::Checkbox("With Side Preview", &side_preview); + if (side_preview) + { + ImGui::SameLine(); + ImGui::Checkbox("With Ref Color", &ref_color); + if (ref_color) + { + ImGui::SameLine(); + ImGui::ColorEdit4("##RefColor", &ref_color_v.x, ImGuiColorEditFlags_NoInputs | misc_flags); + } + } + ImGui::Combo("Display Mode", &display_mode, "Auto/Current\0None\0RGB Only\0HSV Only\0Hex Only\0"); + ImGui::SameLine(); HelpMarker( + "ColorEdit defaults to displaying RGB inputs if you don't specify a display mode, " + "but the user can change it with a right-click on those inputs.\n\nColorPicker defaults to displaying RGB+HSV+Hex " + "if you don't specify a display mode.\n\nYou can change the defaults using SetColorEditOptions()."); + ImGui::SameLine(); HelpMarker("When not specified explicitly (Auto/Current mode), user can right-click the picker to change mode."); + ImGuiColorEditFlags flags = misc_flags; + if (!alpha) flags |= ImGuiColorEditFlags_NoAlpha; // This is by default if you call ColorPicker3() instead of ColorPicker4() + if (alpha_bar) flags |= ImGuiColorEditFlags_AlphaBar; + if (!side_preview) flags |= ImGuiColorEditFlags_NoSidePreview; + if (picker_mode == 1) flags |= ImGuiColorEditFlags_PickerHueBar; + if (picker_mode == 2) flags |= ImGuiColorEditFlags_PickerHueWheel; + if (display_mode == 1) flags |= ImGuiColorEditFlags_NoInputs; // Disable all RGB/HSV/Hex displays + if (display_mode == 2) flags |= ImGuiColorEditFlags_DisplayRGB; // Override display mode + if (display_mode == 3) flags |= ImGuiColorEditFlags_DisplayHSV; + if (display_mode == 4) flags |= ImGuiColorEditFlags_DisplayHex; + ImGui::ColorPicker4("MyColor##4", (float*)&color, flags, ref_color ? &ref_color_v.x : NULL); + + ImGui::Text("Set defaults in code:"); + ImGui::SameLine(); HelpMarker( + "SetColorEditOptions() is designed to allow you to set boot-time default.\n" + "We don't have Push/Pop functions because you can force options on a per-widget basis if needed," + "and the user can change non-forced ones with the options menu.\nWe don't have a getter to avoid" + "encouraging you to persistently save values that aren't forward-compatible."); + if (ImGui::Button("Default: Uint8 + HSV + Hue Bar")) + ImGui::SetColorEditOptions(ImGuiColorEditFlags_Uint8 | ImGuiColorEditFlags_DisplayHSV | ImGuiColorEditFlags_PickerHueBar); + if (ImGui::Button("Default: Float + HDR + Hue Wheel")) + ImGui::SetColorEditOptions(ImGuiColorEditFlags_Float | ImGuiColorEditFlags_HDR | ImGuiColorEditFlags_PickerHueWheel); + + // Always both a small version of both types of pickers (to make it more visible in the demo to people who are skimming quickly through it) + ImGui::Text("Both types:"); + float w = (ImGui::GetContentRegionAvail().x - ImGui::GetStyle().ItemSpacing.y) * 0.40f; + ImGui::SetNextItemWidth(w); + ImGui::ColorPicker3("##MyColor##5", (float*)&color, ImGuiColorEditFlags_PickerHueBar | ImGuiColorEditFlags_NoSidePreview | ImGuiColorEditFlags_NoInputs | ImGuiColorEditFlags_NoAlpha); + ImGui::SameLine(); + ImGui::SetNextItemWidth(w); + ImGui::ColorPicker3("##MyColor##6", (float*)&color, ImGuiColorEditFlags_PickerHueWheel | ImGuiColorEditFlags_NoSidePreview | ImGuiColorEditFlags_NoInputs | ImGuiColorEditFlags_NoAlpha); + + // HSV encoded support (to avoid RGB<>HSV round trips and singularities when S==0 or V==0) + static ImVec4 color_hsv(0.23f, 1.0f, 1.0f, 1.0f); // Stored as HSV! + ImGui::Spacing(); + ImGui::Text("HSV encoded colors"); + ImGui::SameLine(); HelpMarker( + "By default, colors are given to ColorEdit and ColorPicker in RGB, but ImGuiColorEditFlags_InputHSV" + "allows you to store colors as HSV and pass them to ColorEdit and ColorPicker as HSV. This comes with the" + "added benefit that you can manipulate hue values with the picker even when saturation or value are zero."); + ImGui::Text("Color widget with InputHSV:"); + ImGui::ColorEdit4("HSV shown as RGB##1", (float*)&color_hsv, ImGuiColorEditFlags_DisplayRGB | ImGuiColorEditFlags_InputHSV | ImGuiColorEditFlags_Float); + ImGui::ColorEdit4("HSV shown as HSV##1", (float*)&color_hsv, ImGuiColorEditFlags_DisplayHSV | ImGuiColorEditFlags_InputHSV | ImGuiColorEditFlags_Float); + ImGui::DragFloat4("Raw HSV values", (float*)&color_hsv, 0.01f, 0.0f, 1.0f); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Drag and Slider Flags"); + if (ImGui::TreeNode("Drag/Slider Flags")) + { + // Demonstrate using advanced flags for DragXXX and SliderXXX functions. Note that the flags are the same! + static ImGuiSliderFlags flags = ImGuiSliderFlags_None; + ImGui::CheckboxFlags("ImGuiSliderFlags_AlwaysClamp", &flags, ImGuiSliderFlags_AlwaysClamp); + ImGui::SameLine(); HelpMarker("Always clamp value to min/max bounds (if any) when input manually with CTRL+Click."); + ImGui::CheckboxFlags("ImGuiSliderFlags_Logarithmic", &flags, ImGuiSliderFlags_Logarithmic); + ImGui::SameLine(); HelpMarker("Enable logarithmic editing (more precision for small values)."); + ImGui::CheckboxFlags("ImGuiSliderFlags_NoRoundToFormat", &flags, ImGuiSliderFlags_NoRoundToFormat); + ImGui::SameLine(); HelpMarker("Disable rounding underlying value to match precision of the format string (e.g. %.3f values are rounded to those 3 digits)."); + ImGui::CheckboxFlags("ImGuiSliderFlags_NoInput", &flags, ImGuiSliderFlags_NoInput); + ImGui::SameLine(); HelpMarker("Disable CTRL+Click or Enter key allowing to input text directly into the widget."); + + // Drags + static float drag_f = 0.5f; + static int drag_i = 50; + ImGui::Text("Underlying float value: %f", drag_f); + ImGui::DragFloat("DragFloat (0 -> 1)", &drag_f, 0.005f, 0.0f, 1.0f, "%.3f", flags); + ImGui::DragFloat("DragFloat (0 -> +inf)", &drag_f, 0.005f, 0.0f, FLT_MAX, "%.3f", flags); + ImGui::DragFloat("DragFloat (-inf -> 1)", &drag_f, 0.005f, -FLT_MAX, 1.0f, "%.3f", flags); + ImGui::DragFloat("DragFloat (-inf -> +inf)", &drag_f, 0.005f, -FLT_MAX, +FLT_MAX, "%.3f", flags); + ImGui::DragInt("DragInt (0 -> 100)", &drag_i, 0.5f, 0, 100, "%d", flags); + + // Sliders + static float slider_f = 0.5f; + static int slider_i = 50; + ImGui::Text("Underlying float value: %f", slider_f); + ImGui::SliderFloat("SliderFloat (0 -> 1)", &slider_f, 0.0f, 1.0f, "%.3f", flags); + ImGui::SliderInt("SliderInt (0 -> 100)", &slider_i, 0, 100, "%d", flags); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Range Widgets"); + if (ImGui::TreeNode("Range Widgets")) + { + static float begin = 10, end = 90; + static int begin_i = 100, end_i = 1000; + ImGui::DragFloatRange2("range float", &begin, &end, 0.25f, 0.0f, 100.0f, "Min: %.1f %%", "Max: %.1f %%", ImGuiSliderFlags_AlwaysClamp); + ImGui::DragIntRange2("range int", &begin_i, &end_i, 5, 0, 1000, "Min: %d units", "Max: %d units"); + ImGui::DragIntRange2("range int (no bounds)", &begin_i, &end_i, 5, 0, 0, "Min: %d units", "Max: %d units"); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Data Types"); + if (ImGui::TreeNode("Data Types")) + { + // DragScalar/InputScalar/SliderScalar functions allow various data types + // - signed/unsigned + // - 8/16/32/64-bits + // - integer/float/double + // To avoid polluting the public API with all possible combinations, we use the ImGuiDataType enum + // to pass the type, and passing all arguments by pointer. + // This is the reason the test code below creates local variables to hold "zero" "one" etc. for each type. + // In practice, if you frequently use a given type that is not covered by the normal API entry points, + // you can wrap it yourself inside a 1 line function which can take typed argument as value instead of void*, + // and then pass their address to the generic function. For example: + // bool MySliderU64(const char *label, u64* value, u64 min = 0, u64 max = 0, const char* format = "%lld") + // { + // return SliderScalar(label, ImGuiDataType_U64, value, &min, &max, format); + // } + + // Setup limits (as helper variables so we can take their address, as explained above) + // Note: SliderScalar() functions have a maximum usable range of half the natural type maximum, hence the /2. + #ifndef LLONG_MIN + ImS64 LLONG_MIN = -9223372036854775807LL - 1; + ImS64 LLONG_MAX = 9223372036854775807LL; + ImU64 ULLONG_MAX = (2ULL * 9223372036854775807LL + 1); + #endif + const char s8_zero = 0, s8_one = 1, s8_fifty = 50, s8_min = -128, s8_max = 127; + const ImU8 u8_zero = 0, u8_one = 1, u8_fifty = 50, u8_min = 0, u8_max = 255; + const short s16_zero = 0, s16_one = 1, s16_fifty = 50, s16_min = -32768, s16_max = 32767; + const ImU16 u16_zero = 0, u16_one = 1, u16_fifty = 50, u16_min = 0, u16_max = 65535; + const ImS32 s32_zero = 0, s32_one = 1, s32_fifty = 50, s32_min = INT_MIN/2, s32_max = INT_MAX/2, s32_hi_a = INT_MAX/2 - 100, s32_hi_b = INT_MAX/2; + const ImU32 u32_zero = 0, u32_one = 1, u32_fifty = 50, u32_min = 0, u32_max = UINT_MAX/2, u32_hi_a = UINT_MAX/2 - 100, u32_hi_b = UINT_MAX/2; + const ImS64 s64_zero = 0, s64_one = 1, s64_fifty = 50, s64_min = LLONG_MIN/2, s64_max = LLONG_MAX/2, s64_hi_a = LLONG_MAX/2 - 100, s64_hi_b = LLONG_MAX/2; + const ImU64 u64_zero = 0, u64_one = 1, u64_fifty = 50, u64_min = 0, u64_max = ULLONG_MAX/2, u64_hi_a = ULLONG_MAX/2 - 100, u64_hi_b = ULLONG_MAX/2; + const float f32_zero = 0.f, f32_one = 1.f, f32_lo_a = -10000000000.0f, f32_hi_a = +10000000000.0f; + const double f64_zero = 0., f64_one = 1., f64_lo_a = -1000000000000000.0, f64_hi_a = +1000000000000000.0; + + // State + static char s8_v = 127; + static ImU8 u8_v = 255; + static short s16_v = 32767; + static ImU16 u16_v = 65535; + static ImS32 s32_v = -1; + static ImU32 u32_v = (ImU32)-1; + static ImS64 s64_v = -1; + static ImU64 u64_v = (ImU64)-1; + static float f32_v = 0.123f; + static double f64_v = 90000.01234567890123456789; + + const float drag_speed = 0.2f; + static bool drag_clamp = false; + IMGUI_DEMO_MARKER("Widgets/Data Types/Drags"); + ImGui::SeparatorText("Drags"); + ImGui::Checkbox("Clamp integers to 0..50", &drag_clamp); + ImGui::SameLine(); HelpMarker( + "As with every widget in dear imgui, we never modify values unless there is a user interaction.\n" + "You can override the clamping limits by using CTRL+Click to input a value."); + ImGui::DragScalar("drag s8", ImGuiDataType_S8, &s8_v, drag_speed, drag_clamp ? &s8_zero : NULL, drag_clamp ? &s8_fifty : NULL); + ImGui::DragScalar("drag u8", ImGuiDataType_U8, &u8_v, drag_speed, drag_clamp ? &u8_zero : NULL, drag_clamp ? &u8_fifty : NULL, "%u ms"); + ImGui::DragScalar("drag s16", ImGuiDataType_S16, &s16_v, drag_speed, drag_clamp ? &s16_zero : NULL, drag_clamp ? &s16_fifty : NULL); + ImGui::DragScalar("drag u16", ImGuiDataType_U16, &u16_v, drag_speed, drag_clamp ? &u16_zero : NULL, drag_clamp ? &u16_fifty : NULL, "%u ms"); + ImGui::DragScalar("drag s32", ImGuiDataType_S32, &s32_v, drag_speed, drag_clamp ? &s32_zero : NULL, drag_clamp ? &s32_fifty : NULL); + ImGui::DragScalar("drag s32 hex", ImGuiDataType_S32, &s32_v, drag_speed, drag_clamp ? &s32_zero : NULL, drag_clamp ? &s32_fifty : NULL, "0x%08X"); + ImGui::DragScalar("drag u32", ImGuiDataType_U32, &u32_v, drag_speed, drag_clamp ? &u32_zero : NULL, drag_clamp ? &u32_fifty : NULL, "%u ms"); + ImGui::DragScalar("drag s64", ImGuiDataType_S64, &s64_v, drag_speed, drag_clamp ? &s64_zero : NULL, drag_clamp ? &s64_fifty : NULL); + ImGui::DragScalar("drag u64", ImGuiDataType_U64, &u64_v, drag_speed, drag_clamp ? &u64_zero : NULL, drag_clamp ? &u64_fifty : NULL); + ImGui::DragScalar("drag float", ImGuiDataType_Float, &f32_v, 0.005f, &f32_zero, &f32_one, "%f"); + ImGui::DragScalar("drag float log", ImGuiDataType_Float, &f32_v, 0.005f, &f32_zero, &f32_one, "%f", ImGuiSliderFlags_Logarithmic); + ImGui::DragScalar("drag double", ImGuiDataType_Double, &f64_v, 0.0005f, &f64_zero, NULL, "%.10f grams"); + ImGui::DragScalar("drag double log",ImGuiDataType_Double, &f64_v, 0.0005f, &f64_zero, &f64_one, "0 < %.10f < 1", ImGuiSliderFlags_Logarithmic); + + IMGUI_DEMO_MARKER("Widgets/Data Types/Sliders"); + ImGui::SeparatorText("Sliders"); + ImGui::SliderScalar("slider s8 full", ImGuiDataType_S8, &s8_v, &s8_min, &s8_max, "%d"); + ImGui::SliderScalar("slider u8 full", ImGuiDataType_U8, &u8_v, &u8_min, &u8_max, "%u"); + ImGui::SliderScalar("slider s16 full", ImGuiDataType_S16, &s16_v, &s16_min, &s16_max, "%d"); + ImGui::SliderScalar("slider u16 full", ImGuiDataType_U16, &u16_v, &u16_min, &u16_max, "%u"); + ImGui::SliderScalar("slider s32 low", ImGuiDataType_S32, &s32_v, &s32_zero, &s32_fifty,"%d"); + ImGui::SliderScalar("slider s32 high", ImGuiDataType_S32, &s32_v, &s32_hi_a, &s32_hi_b, "%d"); + ImGui::SliderScalar("slider s32 full", ImGuiDataType_S32, &s32_v, &s32_min, &s32_max, "%d"); + ImGui::SliderScalar("slider s32 hex", ImGuiDataType_S32, &s32_v, &s32_zero, &s32_fifty, "0x%04X"); + ImGui::SliderScalar("slider u32 low", ImGuiDataType_U32, &u32_v, &u32_zero, &u32_fifty,"%u"); + ImGui::SliderScalar("slider u32 high", ImGuiDataType_U32, &u32_v, &u32_hi_a, &u32_hi_b, "%u"); + ImGui::SliderScalar("slider u32 full", ImGuiDataType_U32, &u32_v, &u32_min, &u32_max, "%u"); + ImGui::SliderScalar("slider s64 low", ImGuiDataType_S64, &s64_v, &s64_zero, &s64_fifty,"%" PRId64); + ImGui::SliderScalar("slider s64 high", ImGuiDataType_S64, &s64_v, &s64_hi_a, &s64_hi_b, "%" PRId64); + ImGui::SliderScalar("slider s64 full", ImGuiDataType_S64, &s64_v, &s64_min, &s64_max, "%" PRId64); + ImGui::SliderScalar("slider u64 low", ImGuiDataType_U64, &u64_v, &u64_zero, &u64_fifty,"%" PRIu64 " ms"); + ImGui::SliderScalar("slider u64 high", ImGuiDataType_U64, &u64_v, &u64_hi_a, &u64_hi_b, "%" PRIu64 " ms"); + ImGui::SliderScalar("slider u64 full", ImGuiDataType_U64, &u64_v, &u64_min, &u64_max, "%" PRIu64 " ms"); + ImGui::SliderScalar("slider float low", ImGuiDataType_Float, &f32_v, &f32_zero, &f32_one); + ImGui::SliderScalar("slider float low log", ImGuiDataType_Float, &f32_v, &f32_zero, &f32_one, "%.10f", ImGuiSliderFlags_Logarithmic); + ImGui::SliderScalar("slider float high", ImGuiDataType_Float, &f32_v, &f32_lo_a, &f32_hi_a, "%e"); + ImGui::SliderScalar("slider double low", ImGuiDataType_Double, &f64_v, &f64_zero, &f64_one, "%.10f grams"); + ImGui::SliderScalar("slider double low log",ImGuiDataType_Double, &f64_v, &f64_zero, &f64_one, "%.10f", ImGuiSliderFlags_Logarithmic); + ImGui::SliderScalar("slider double high", ImGuiDataType_Double, &f64_v, &f64_lo_a, &f64_hi_a, "%e grams"); + + ImGui::SeparatorText("Sliders (reverse)"); + ImGui::SliderScalar("slider s8 reverse", ImGuiDataType_S8, &s8_v, &s8_max, &s8_min, "%d"); + ImGui::SliderScalar("slider u8 reverse", ImGuiDataType_U8, &u8_v, &u8_max, &u8_min, "%u"); + ImGui::SliderScalar("slider s32 reverse", ImGuiDataType_S32, &s32_v, &s32_fifty, &s32_zero, "%d"); + ImGui::SliderScalar("slider u32 reverse", ImGuiDataType_U32, &u32_v, &u32_fifty, &u32_zero, "%u"); + ImGui::SliderScalar("slider s64 reverse", ImGuiDataType_S64, &s64_v, &s64_fifty, &s64_zero, "%" PRId64); + ImGui::SliderScalar("slider u64 reverse", ImGuiDataType_U64, &u64_v, &u64_fifty, &u64_zero, "%" PRIu64 " ms"); + + IMGUI_DEMO_MARKER("Widgets/Data Types/Inputs"); + static bool inputs_step = true; + ImGui::SeparatorText("Inputs"); + ImGui::Checkbox("Show step buttons", &inputs_step); + ImGui::InputScalar("input s8", ImGuiDataType_S8, &s8_v, inputs_step ? &s8_one : NULL, NULL, "%d"); + ImGui::InputScalar("input u8", ImGuiDataType_U8, &u8_v, inputs_step ? &u8_one : NULL, NULL, "%u"); + ImGui::InputScalar("input s16", ImGuiDataType_S16, &s16_v, inputs_step ? &s16_one : NULL, NULL, "%d"); + ImGui::InputScalar("input u16", ImGuiDataType_U16, &u16_v, inputs_step ? &u16_one : NULL, NULL, "%u"); + ImGui::InputScalar("input s32", ImGuiDataType_S32, &s32_v, inputs_step ? &s32_one : NULL, NULL, "%d"); + ImGui::InputScalar("input s32 hex", ImGuiDataType_S32, &s32_v, inputs_step ? &s32_one : NULL, NULL, "%04X"); + ImGui::InputScalar("input u32", ImGuiDataType_U32, &u32_v, inputs_step ? &u32_one : NULL, NULL, "%u"); + ImGui::InputScalar("input u32 hex", ImGuiDataType_U32, &u32_v, inputs_step ? &u32_one : NULL, NULL, "%08X"); + ImGui::InputScalar("input s64", ImGuiDataType_S64, &s64_v, inputs_step ? &s64_one : NULL); + ImGui::InputScalar("input u64", ImGuiDataType_U64, &u64_v, inputs_step ? &u64_one : NULL); + ImGui::InputScalar("input float", ImGuiDataType_Float, &f32_v, inputs_step ? &f32_one : NULL); + ImGui::InputScalar("input double", ImGuiDataType_Double, &f64_v, inputs_step ? &f64_one : NULL); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Multi-component Widgets"); + if (ImGui::TreeNode("Multi-component Widgets")) + { + static float vec4f[4] = { 0.10f, 0.20f, 0.30f, 0.44f }; + static int vec4i[4] = { 1, 5, 100, 255 }; + + ImGui::SeparatorText("2-wide"); + ImGui::InputFloat2("input float2", vec4f); + ImGui::DragFloat2("drag float2", vec4f, 0.01f, 0.0f, 1.0f); + ImGui::SliderFloat2("slider float2", vec4f, 0.0f, 1.0f); + ImGui::InputInt2("input int2", vec4i); + ImGui::DragInt2("drag int2", vec4i, 1, 0, 255); + ImGui::SliderInt2("slider int2", vec4i, 0, 255); + + ImGui::SeparatorText("3-wide"); + ImGui::InputFloat3("input float3", vec4f); + ImGui::DragFloat3("drag float3", vec4f, 0.01f, 0.0f, 1.0f); + ImGui::SliderFloat3("slider float3", vec4f, 0.0f, 1.0f); + ImGui::InputInt3("input int3", vec4i); + ImGui::DragInt3("drag int3", vec4i, 1, 0, 255); + ImGui::SliderInt3("slider int3", vec4i, 0, 255); + + ImGui::SeparatorText("4-wide"); + ImGui::InputFloat4("input float4", vec4f); + ImGui::DragFloat4("drag float4", vec4f, 0.01f, 0.0f, 1.0f); + ImGui::SliderFloat4("slider float4", vec4f, 0.0f, 1.0f); + ImGui::InputInt4("input int4", vec4i); + ImGui::DragInt4("drag int4", vec4i, 1, 0, 255); + ImGui::SliderInt4("slider int4", vec4i, 0, 255); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Vertical Sliders"); + if (ImGui::TreeNode("Vertical Sliders")) + { + const float spacing = 4; + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(spacing, spacing)); + + static int int_value = 0; + ImGui::VSliderInt("##int", ImVec2(18, 160), &int_value, 0, 5); + ImGui::SameLine(); + + static float values[7] = { 0.0f, 0.60f, 0.35f, 0.9f, 0.70f, 0.20f, 0.0f }; + ImGui::PushID("set1"); + for (int i = 0; i < 7; i++) + { + if (i > 0) ImGui::SameLine(); + ImGui::PushID(i); + ImGui::PushStyleColor(ImGuiCol_FrameBg, (ImVec4)ImColor::HSV(i / 7.0f, 0.5f, 0.5f)); + ImGui::PushStyleColor(ImGuiCol_FrameBgHovered, (ImVec4)ImColor::HSV(i / 7.0f, 0.6f, 0.5f)); + ImGui::PushStyleColor(ImGuiCol_FrameBgActive, (ImVec4)ImColor::HSV(i / 7.0f, 0.7f, 0.5f)); + ImGui::PushStyleColor(ImGuiCol_SliderGrab, (ImVec4)ImColor::HSV(i / 7.0f, 0.9f, 0.9f)); + ImGui::VSliderFloat("##v", ImVec2(18, 160), &values[i], 0.0f, 1.0f, ""); + if (ImGui::IsItemActive() || ImGui::IsItemHovered()) + ImGui::SetTooltip("%.3f", values[i]); + ImGui::PopStyleColor(4); + ImGui::PopID(); + } + ImGui::PopID(); + + ImGui::SameLine(); + ImGui::PushID("set2"); + static float values2[4] = { 0.20f, 0.80f, 0.40f, 0.25f }; + const int rows = 3; + const ImVec2 small_slider_size(18, (float)(int)((160.0f - (rows - 1) * spacing) / rows)); + for (int nx = 0; nx < 4; nx++) + { + if (nx > 0) ImGui::SameLine(); + ImGui::BeginGroup(); + for (int ny = 0; ny < rows; ny++) + { + ImGui::PushID(nx * rows + ny); + ImGui::VSliderFloat("##v", small_slider_size, &values2[nx], 0.0f, 1.0f, ""); + if (ImGui::IsItemActive() || ImGui::IsItemHovered()) + ImGui::SetTooltip("%.3f", values2[nx]); + ImGui::PopID(); + } + ImGui::EndGroup(); + } + ImGui::PopID(); + + ImGui::SameLine(); + ImGui::PushID("set3"); + for (int i = 0; i < 4; i++) + { + if (i > 0) ImGui::SameLine(); + ImGui::PushID(i); + ImGui::PushStyleVar(ImGuiStyleVar_GrabMinSize, 40); + ImGui::VSliderFloat("##v", ImVec2(40, 160), &values[i], 0.0f, 1.0f, "%.2f\nsec"); + ImGui::PopStyleVar(); + ImGui::PopID(); + } + ImGui::PopID(); + ImGui::PopStyleVar(); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Drag and drop"); + if (ImGui::TreeNode("Drag and Drop")) + { + IMGUI_DEMO_MARKER("Widgets/Drag and drop/Standard widgets"); + if (ImGui::TreeNode("Drag and drop in standard widgets")) + { + // ColorEdit widgets automatically act as drag source and drag target. + // They are using standardized payload strings IMGUI_PAYLOAD_TYPE_COLOR_3F and IMGUI_PAYLOAD_TYPE_COLOR_4F + // to allow your own widgets to use colors in their drag and drop interaction. + // Also see 'Demo->Widgets->Color/Picker Widgets->Palette' demo. + HelpMarker("You can drag from the color squares."); + static float col1[3] = { 1.0f, 0.0f, 0.2f }; + static float col2[4] = { 0.4f, 0.7f, 0.0f, 0.5f }; + ImGui::ColorEdit3("color 1", col1); + ImGui::ColorEdit4("color 2", col2); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Drag and drop/Copy-swap items"); + if (ImGui::TreeNode("Drag and drop to copy/swap items")) + { + enum Mode + { + Mode_Copy, + Mode_Move, + Mode_Swap + }; + static int mode = 0; + if (ImGui::RadioButton("Copy", mode == Mode_Copy)) { mode = Mode_Copy; } ImGui::SameLine(); + if (ImGui::RadioButton("Move", mode == Mode_Move)) { mode = Mode_Move; } ImGui::SameLine(); + if (ImGui::RadioButton("Swap", mode == Mode_Swap)) { mode = Mode_Swap; } + static const char* names[9] = + { + "Bobby", "Beatrice", "Betty", + "Brianna", "Barry", "Bernard", + "Bibi", "Blaine", "Bryn" + }; + for (int n = 0; n < IM_ARRAYSIZE(names); n++) + { + ImGui::PushID(n); + if ((n % 3) != 0) + ImGui::SameLine(); + ImGui::Button(names[n], ImVec2(60, 60)); + + // Our buttons are both drag sources and drag targets here! + if (ImGui::BeginDragDropSource(ImGuiDragDropFlags_None)) + { + // Set payload to carry the index of our item (could be anything) + ImGui::SetDragDropPayload("DND_DEMO_CELL", &n, sizeof(int)); + + // Display preview (could be anything, e.g. when dragging an image we could decide to display + // the filename and a small preview of the image, etc.) + if (mode == Mode_Copy) { ImGui::Text("Copy %s", names[n]); } + if (mode == Mode_Move) { ImGui::Text("Move %s", names[n]); } + if (mode == Mode_Swap) { ImGui::Text("Swap %s", names[n]); } + ImGui::EndDragDropSource(); + } + if (ImGui::BeginDragDropTarget()) + { + if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload("DND_DEMO_CELL")) + { + IM_ASSERT(payload->DataSize == sizeof(int)); + int payload_n = *(const int*)payload->Data; + if (mode == Mode_Copy) + { + names[n] = names[payload_n]; + } + if (mode == Mode_Move) + { + names[n] = names[payload_n]; + names[payload_n] = ""; + } + if (mode == Mode_Swap) + { + const char* tmp = names[n]; + names[n] = names[payload_n]; + names[payload_n] = tmp; + } + } + ImGui::EndDragDropTarget(); + } + ImGui::PopID(); + } + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Drag and Drop/Drag to reorder items (simple)"); + if (ImGui::TreeNode("Drag to reorder items (simple)")) + { + // Simple reordering + HelpMarker( + "We don't use the drag and drop api at all here! " + "Instead we query when the item is held but not hovered, and order items accordingly."); + static const char* item_names[] = { "Item One", "Item Two", "Item Three", "Item Four", "Item Five" }; + for (int n = 0; n < IM_ARRAYSIZE(item_names); n++) + { + const char* item = item_names[n]; + ImGui::Selectable(item); + + if (ImGui::IsItemActive() && !ImGui::IsItemHovered()) + { + int n_next = n + (ImGui::GetMouseDragDelta(0).y < 0.f ? -1 : 1); + if (n_next >= 0 && n_next < IM_ARRAYSIZE(item_names)) + { + item_names[n] = item_names[n_next]; + item_names[n_next] = item; + ImGui::ResetMouseDragDelta(); + } + } + } + ImGui::TreePop(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Querying Item Status (Edited,Active,Hovered etc.)"); + if (ImGui::TreeNode("Querying Item Status (Edited/Active/Hovered etc.)")) + { + // Select an item type + const char* item_names[] = + { + "Text", "Button", "Button (w/ repeat)", "Checkbox", "SliderFloat", "InputText", "InputTextMultiline", "InputFloat", + "InputFloat3", "ColorEdit4", "Selectable", "MenuItem", "TreeNode", "TreeNode (w/ double-click)", "Combo", "ListBox" + }; + static int item_type = 4; + static bool item_disabled = false; + ImGui::Combo("Item Type", &item_type, item_names, IM_ARRAYSIZE(item_names), IM_ARRAYSIZE(item_names)); + ImGui::SameLine(); + HelpMarker("Testing how various types of items are interacting with the IsItemXXX functions. Note that the bool return value of most ImGui function is generally equivalent to calling ImGui::IsItemHovered()."); + ImGui::Checkbox("Item Disabled", &item_disabled); + + // Submit selected items so we can query their status in the code following it. + bool ret = false; + static bool b = false; + static float col4f[4] = { 1.0f, 0.5, 0.0f, 1.0f }; + static char str[16] = {}; + if (item_disabled) + ImGui::BeginDisabled(true); + if (item_type == 0) { ImGui::Text("ITEM: Text"); } // Testing text items with no identifier/interaction + if (item_type == 1) { ret = ImGui::Button("ITEM: Button"); } // Testing button + if (item_type == 2) { ImGui::PushButtonRepeat(true); ret = ImGui::Button("ITEM: Button"); ImGui::PopButtonRepeat(); } // Testing button (with repeater) + if (item_type == 3) { ret = ImGui::Checkbox("ITEM: Checkbox", &b); } // Testing checkbox + if (item_type == 4) { ret = ImGui::SliderFloat("ITEM: SliderFloat", &col4f[0], 0.0f, 1.0f); } // Testing basic item + if (item_type == 5) { ret = ImGui::InputText("ITEM: InputText", &str[0], IM_ARRAYSIZE(str)); } // Testing input text (which handles tabbing) + if (item_type == 6) { ret = ImGui::InputTextMultiline("ITEM: InputTextMultiline", &str[0], IM_ARRAYSIZE(str)); } // Testing input text (which uses a child window) + if (item_type == 7) { ret = ImGui::InputFloat("ITEM: InputFloat", col4f, 1.0f); } // Testing +/- buttons on scalar input + if (item_type == 8) { ret = ImGui::InputFloat3("ITEM: InputFloat3", col4f); } // Testing multi-component items (IsItemXXX flags are reported merged) + if (item_type == 9) { ret = ImGui::ColorEdit4("ITEM: ColorEdit4", col4f); } // Testing multi-component items (IsItemXXX flags are reported merged) + if (item_type == 10){ ret = ImGui::Selectable("ITEM: Selectable"); } // Testing selectable item + if (item_type == 11){ ret = ImGui::MenuItem("ITEM: MenuItem"); } // Testing menu item (they use ImGuiButtonFlags_PressedOnRelease button policy) + if (item_type == 12){ ret = ImGui::TreeNode("ITEM: TreeNode"); if (ret) ImGui::TreePop(); } // Testing tree node + if (item_type == 13){ ret = ImGui::TreeNodeEx("ITEM: TreeNode w/ ImGuiTreeNodeFlags_OpenOnDoubleClick", ImGuiTreeNodeFlags_OpenOnDoubleClick | ImGuiTreeNodeFlags_NoTreePushOnOpen); } // Testing tree node with ImGuiButtonFlags_PressedOnDoubleClick button policy. + if (item_type == 14){ const char* items[] = { "Apple", "Banana", "Cherry", "Kiwi" }; static int current = 1; ret = ImGui::Combo("ITEM: Combo", ¤t, items, IM_ARRAYSIZE(items)); } + if (item_type == 15){ const char* items[] = { "Apple", "Banana", "Cherry", "Kiwi" }; static int current = 1; ret = ImGui::ListBox("ITEM: ListBox", ¤t, items, IM_ARRAYSIZE(items), IM_ARRAYSIZE(items)); } + + bool hovered_delay_none = ImGui::IsItemHovered(); + bool hovered_delay_stationary = ImGui::IsItemHovered(ImGuiHoveredFlags_Stationary); + bool hovered_delay_short = ImGui::IsItemHovered(ImGuiHoveredFlags_DelayShort); + bool hovered_delay_normal = ImGui::IsItemHovered(ImGuiHoveredFlags_DelayNormal); + bool hovered_delay_tooltip = ImGui::IsItemHovered(ImGuiHoveredFlags_ForTooltip); // = Normal + Stationary + + // Display the values of IsItemHovered() and other common item state functions. + // Note that the ImGuiHoveredFlags_XXX flags can be combined. + // Because BulletText is an item itself and that would affect the output of IsItemXXX functions, + // we query every state in a single call to avoid storing them and to simplify the code. + ImGui::BulletText( + "Return value = %d\n" + "IsItemFocused() = %d\n" + "IsItemHovered() = %d\n" + "IsItemHovered(_AllowWhenBlockedByPopup) = %d\n" + "IsItemHovered(_AllowWhenBlockedByActiveItem) = %d\n" + "IsItemHovered(_AllowWhenOverlappedByItem) = %d\n" + "IsItemHovered(_AllowWhenOverlappedByWindow) = %d\n" + "IsItemHovered(_AllowWhenDisabled) = %d\n" + "IsItemHovered(_RectOnly) = %d\n" + "IsItemActive() = %d\n" + "IsItemEdited() = %d\n" + "IsItemActivated() = %d\n" + "IsItemDeactivated() = %d\n" + "IsItemDeactivatedAfterEdit() = %d\n" + "IsItemVisible() = %d\n" + "IsItemClicked() = %d\n" + "IsItemToggledOpen() = %d\n" + "GetItemRectMin() = (%.1f, %.1f)\n" + "GetItemRectMax() = (%.1f, %.1f)\n" + "GetItemRectSize() = (%.1f, %.1f)", + ret, + ImGui::IsItemFocused(), + ImGui::IsItemHovered(), + ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenBlockedByPopup), + ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenBlockedByActiveItem), + ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenOverlappedByItem), + ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenOverlappedByWindow), + ImGui::IsItemHovered(ImGuiHoveredFlags_AllowWhenDisabled), + ImGui::IsItemHovered(ImGuiHoveredFlags_RectOnly), + ImGui::IsItemActive(), + ImGui::IsItemEdited(), + ImGui::IsItemActivated(), + ImGui::IsItemDeactivated(), + ImGui::IsItemDeactivatedAfterEdit(), + ImGui::IsItemVisible(), + ImGui::IsItemClicked(), + ImGui::IsItemToggledOpen(), + ImGui::GetItemRectMin().x, ImGui::GetItemRectMin().y, + ImGui::GetItemRectMax().x, ImGui::GetItemRectMax().y, + ImGui::GetItemRectSize().x, ImGui::GetItemRectSize().y + ); + ImGui::BulletText( + "with Hovering Delay or Stationary test:\n" + "IsItemHovered() = = %d\n" + "IsItemHovered(_Stationary) = %d\n" + "IsItemHovered(_DelayShort) = %d\n" + "IsItemHovered(_DelayNormal) = %d\n" + "IsItemHovered(_Tooltip) = %d", + hovered_delay_none, hovered_delay_stationary, hovered_delay_short, hovered_delay_normal, hovered_delay_tooltip); + + if (item_disabled) + ImGui::EndDisabled(); + + char buf[1] = ""; + ImGui::InputText("unused", buf, IM_ARRAYSIZE(buf), ImGuiInputTextFlags_ReadOnly); + ImGui::SameLine(); + HelpMarker("This widget is only here to be able to tab-out of the widgets above and see e.g. Deactivated() status."); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Querying Window Status (Focused,Hovered etc.)"); + if (ImGui::TreeNode("Querying Window Status (Focused/Hovered etc.)")) + { + static bool embed_all_inside_a_child_window = false; + ImGui::Checkbox("Embed everything inside a child window for testing _RootWindow flag.", &embed_all_inside_a_child_window); + if (embed_all_inside_a_child_window) + ImGui::BeginChild("outer_child", ImVec2(0, ImGui::GetFontSize() * 20.0f), true); + + // Testing IsWindowFocused() function with its various flags. + ImGui::BulletText( + "IsWindowFocused() = %d\n" + "IsWindowFocused(_ChildWindows) = %d\n" + "IsWindowFocused(_ChildWindows|_NoPopupHierarchy) = %d\n" + "IsWindowFocused(_ChildWindows|_DockHierarchy) = %d\n" + "IsWindowFocused(_ChildWindows|_RootWindow) = %d\n" + "IsWindowFocused(_ChildWindows|_RootWindow|_NoPopupHierarchy) = %d\n" + "IsWindowFocused(_ChildWindows|_RootWindow|_DockHierarchy) = %d\n" + "IsWindowFocused(_RootWindow) = %d\n" + "IsWindowFocused(_RootWindow|_NoPopupHierarchy) = %d\n" + "IsWindowFocused(_RootWindow|_DockHierarchy) = %d\n" + "IsWindowFocused(_AnyWindow) = %d\n", + ImGui::IsWindowFocused(), + ImGui::IsWindowFocused(ImGuiFocusedFlags_ChildWindows), + ImGui::IsWindowFocused(ImGuiFocusedFlags_ChildWindows | ImGuiFocusedFlags_NoPopupHierarchy), + ImGui::IsWindowFocused(ImGuiFocusedFlags_ChildWindows | ImGuiFocusedFlags_DockHierarchy), + ImGui::IsWindowFocused(ImGuiFocusedFlags_ChildWindows | ImGuiFocusedFlags_RootWindow), + ImGui::IsWindowFocused(ImGuiFocusedFlags_ChildWindows | ImGuiFocusedFlags_RootWindow | ImGuiFocusedFlags_NoPopupHierarchy), + ImGui::IsWindowFocused(ImGuiFocusedFlags_ChildWindows | ImGuiFocusedFlags_RootWindow | ImGuiFocusedFlags_DockHierarchy), + ImGui::IsWindowFocused(ImGuiFocusedFlags_RootWindow), + ImGui::IsWindowFocused(ImGuiFocusedFlags_RootWindow | ImGuiFocusedFlags_NoPopupHierarchy), + ImGui::IsWindowFocused(ImGuiFocusedFlags_RootWindow | ImGuiFocusedFlags_DockHierarchy), + ImGui::IsWindowFocused(ImGuiFocusedFlags_AnyWindow)); + + // Testing IsWindowHovered() function with its various flags. + ImGui::BulletText( + "IsWindowHovered() = %d\n" + "IsWindowHovered(_AllowWhenBlockedByPopup) = %d\n" + "IsWindowHovered(_AllowWhenBlockedByActiveItem) = %d\n" + "IsWindowHovered(_ChildWindows) = %d\n" + "IsWindowHovered(_ChildWindows|_NoPopupHierarchy) = %d\n" + "IsWindowHovered(_ChildWindows|_DockHierarchy) = %d\n" + "IsWindowHovered(_ChildWindows|_RootWindow) = %d\n" + "IsWindowHovered(_ChildWindows|_RootWindow|_NoPopupHierarchy) = %d\n" + "IsWindowHovered(_ChildWindows|_RootWindow|_DockHierarchy) = %d\n" + "IsWindowHovered(_RootWindow) = %d\n" + "IsWindowHovered(_RootWindow|_NoPopupHierarchy) = %d\n" + "IsWindowHovered(_RootWindow|_DockHierarchy) = %d\n" + "IsWindowHovered(_ChildWindows|_AllowWhenBlockedByPopup) = %d\n" + "IsWindowHovered(_AnyWindow) = %d\n" + "IsWindowHovered(_Stationary) = %d\n", + ImGui::IsWindowHovered(), + ImGui::IsWindowHovered(ImGuiHoveredFlags_AllowWhenBlockedByPopup), + ImGui::IsWindowHovered(ImGuiHoveredFlags_AllowWhenBlockedByActiveItem), + ImGui::IsWindowHovered(ImGuiHoveredFlags_ChildWindows), + ImGui::IsWindowHovered(ImGuiHoveredFlags_ChildWindows | ImGuiHoveredFlags_NoPopupHierarchy), + ImGui::IsWindowHovered(ImGuiHoveredFlags_ChildWindows | ImGuiHoveredFlags_DockHierarchy), + ImGui::IsWindowHovered(ImGuiHoveredFlags_ChildWindows | ImGuiHoveredFlags_RootWindow), + ImGui::IsWindowHovered(ImGuiHoveredFlags_ChildWindows | ImGuiHoveredFlags_RootWindow | ImGuiHoveredFlags_NoPopupHierarchy), + ImGui::IsWindowHovered(ImGuiHoveredFlags_ChildWindows | ImGuiHoveredFlags_RootWindow | ImGuiHoveredFlags_DockHierarchy), + ImGui::IsWindowHovered(ImGuiHoveredFlags_RootWindow), + ImGui::IsWindowHovered(ImGuiHoveredFlags_RootWindow | ImGuiHoveredFlags_NoPopupHierarchy), + ImGui::IsWindowHovered(ImGuiHoveredFlags_RootWindow | ImGuiHoveredFlags_DockHierarchy), + ImGui::IsWindowHovered(ImGuiHoveredFlags_ChildWindows | ImGuiHoveredFlags_AllowWhenBlockedByPopup), + ImGui::IsWindowHovered(ImGuiHoveredFlags_AnyWindow), + ImGui::IsWindowHovered(ImGuiHoveredFlags_Stationary)); + + ImGui::BeginChild("child", ImVec2(0, 50), true); + ImGui::Text("This is another child window for testing the _ChildWindows flag."); + ImGui::EndChild(); + if (embed_all_inside_a_child_window) + ImGui::EndChild(); + + // Calling IsItemHovered() after begin returns the hovered status of the title bar. + // This is useful in particular if you want to create a context menu associated to the title bar of a window. + // This will also work when docked into a Tab (the Tab replace the Title Bar and guarantee the same properties). + static bool test_window = false; + ImGui::Checkbox("Hovered/Active tests after Begin() for title bar testing", &test_window); + if (test_window) + { + // FIXME-DOCK: This window cannot be docked within the ImGui Demo window, this will cause a feedback loop and get them stuck. + // Could we fix this through an ImGuiWindowClass feature? Or an API call to tag our parent as "don't skip items"? + ImGui::Begin("Title bar Hovered/Active tests", &test_window); + if (ImGui::BeginPopupContextItem()) // <-- This is using IsItemHovered() + { + if (ImGui::MenuItem("Close")) { test_window = false; } + ImGui::EndPopup(); + } + ImGui::Text( + "IsItemHovered() after begin = %d (== is title bar hovered)\n" + "IsItemActive() after begin = %d (== is window being clicked/moved)\n", + ImGui::IsItemHovered(), ImGui::IsItemActive()); + ImGui::End(); + } + + ImGui::TreePop(); + } + + // Demonstrate BeginDisabled/EndDisabled using a checkbox located at the bottom of the section (which is a bit odd: + // logically we'd have this checkbox at the top of the section, but we don't want this feature to steal that space) + if (disable_all) + ImGui::EndDisabled(); + + IMGUI_DEMO_MARKER("Widgets/Disable Block"); + if (ImGui::TreeNode("Disable block")) + { + ImGui::Checkbox("Disable entire section above", &disable_all); + ImGui::SameLine(); HelpMarker("Demonstrate using BeginDisabled()/EndDisabled() across this section."); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Widgets/Text Filter"); + if (ImGui::TreeNode("Text Filter")) + { + // Helper class to easy setup a text filter. + // You may want to implement a more feature-full filtering scheme in your own application. + HelpMarker("Not a widget per-se, but ImGuiTextFilter is a helper to perform simple filtering on text strings."); + static ImGuiTextFilter filter; + ImGui::Text("Filter usage:\n" + " \"\" display all lines\n" + " \"xxx\" display lines containing \"xxx\"\n" + " \"xxx,yyy\" display lines containing \"xxx\" or \"yyy\"\n" + " \"-xxx\" hide lines containing \"xxx\""); + filter.Draw(); + const char* lines[] = { "aaa1.c", "bbb1.c", "ccc1.c", "aaa2.cpp", "bbb2.cpp", "ccc2.cpp", "abc.h", "hello, world" }; + for (int i = 0; i < IM_ARRAYSIZE(lines); i++) + if (filter.PassFilter(lines[i])) + ImGui::BulletText("%s", lines[i]); + ImGui::TreePop(); + } +} + +static void ShowDemoWindowLayout() +{ + IMGUI_DEMO_MARKER("Layout"); + if (!ImGui::CollapsingHeader("Layout & Scrolling")) + return; + + IMGUI_DEMO_MARKER("Layout/Child windows"); + if (ImGui::TreeNode("Child windows")) + { + ImGui::SeparatorText("Child windows"); + + HelpMarker("Use child windows to begin into a self-contained independent scrolling/clipping regions within a host window."); + static bool disable_mouse_wheel = false; + static bool disable_menu = false; + ImGui::Checkbox("Disable Mouse Wheel", &disable_mouse_wheel); + ImGui::Checkbox("Disable Menu", &disable_menu); + + // Child 1: no border, enable horizontal scrollbar + { + ImGuiWindowFlags window_flags = ImGuiWindowFlags_HorizontalScrollbar; + if (disable_mouse_wheel) + window_flags |= ImGuiWindowFlags_NoScrollWithMouse; + ImGui::BeginChild("ChildL", ImVec2(ImGui::GetContentRegionAvail().x * 0.5f, 260), false, window_flags); + for (int i = 0; i < 100; i++) + ImGui::Text("%04d: scrollable region", i); + ImGui::EndChild(); + } + + ImGui::SameLine(); + + // Child 2: rounded border + { + ImGuiWindowFlags window_flags = ImGuiWindowFlags_None; + if (disable_mouse_wheel) + window_flags |= ImGuiWindowFlags_NoScrollWithMouse; + if (!disable_menu) + window_flags |= ImGuiWindowFlags_MenuBar; + ImGui::PushStyleVar(ImGuiStyleVar_ChildRounding, 5.0f); + ImGui::BeginChild("ChildR", ImVec2(0, 260), true, window_flags); + if (!disable_menu && ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("Menu")) + { + ShowExampleMenuFile(); + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + if (ImGui::BeginTable("split", 2, ImGuiTableFlags_Resizable | ImGuiTableFlags_NoSavedSettings)) + { + for (int i = 0; i < 100; i++) + { + char buf[32]; + sprintf(buf, "%03d", i); + ImGui::TableNextColumn(); + ImGui::Button(buf, ImVec2(-FLT_MIN, 0.0f)); + } + ImGui::EndTable(); + } + ImGui::EndChild(); + ImGui::PopStyleVar(); + } + + ImGui::SeparatorText("Misc/Advanced"); + + // Demonstrate a few extra things + // - Changing ImGuiCol_ChildBg (which is transparent black in default styles) + // - Using SetCursorPos() to position child window (the child window is an item from the POV of parent window) + // You can also call SetNextWindowPos() to position the child window. The parent window will effectively + // layout from this position. + // - Using ImGui::GetItemRectMin/Max() to query the "item" state (because the child window is an item from + // the POV of the parent window). See 'Demo->Querying Status (Edited/Active/Hovered etc.)' for details. + { + static int offset_x = 0; + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8); + ImGui::DragInt("Offset X", &offset_x, 1.0f, -1000, 1000); + + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + (float)offset_x); + ImGui::PushStyleColor(ImGuiCol_ChildBg, IM_COL32(255, 0, 0, 100)); + ImGui::BeginChild("Red", ImVec2(200, 100), true, ImGuiWindowFlags_None); + for (int n = 0; n < 50; n++) + ImGui::Text("Some test %d", n); + ImGui::EndChild(); + bool child_is_hovered = ImGui::IsItemHovered(); + ImVec2 child_rect_min = ImGui::GetItemRectMin(); + ImVec2 child_rect_max = ImGui::GetItemRectMax(); + ImGui::PopStyleColor(); + ImGui::Text("Hovered: %d", child_is_hovered); + ImGui::Text("Rect of child window is: (%.0f,%.0f) (%.0f,%.0f)", child_rect_min.x, child_rect_min.y, child_rect_max.x, child_rect_max.y); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Layout/Widgets Width"); + if (ImGui::TreeNode("Widgets Width")) + { + static float f = 0.0f; + static bool show_indented_items = true; + ImGui::Checkbox("Show indented items", &show_indented_items); + + // Use SetNextItemWidth() to set the width of a single upcoming item. + // Use PushItemWidth()/PopItemWidth() to set the width of a group of items. + // In real code use you'll probably want to choose width values that are proportional to your font size + // e.g. Using '20.0f * GetFontSize()' as width instead of '200.0f', etc. + + ImGui::Text("SetNextItemWidth/PushItemWidth(100)"); + ImGui::SameLine(); HelpMarker("Fixed width."); + ImGui::PushItemWidth(100); + ImGui::DragFloat("float##1b", &f); + if (show_indented_items) + { + ImGui::Indent(); + ImGui::DragFloat("float (indented)##1b", &f); + ImGui::Unindent(); + } + ImGui::PopItemWidth(); + + ImGui::Text("SetNextItemWidth/PushItemWidth(-100)"); + ImGui::SameLine(); HelpMarker("Align to right edge minus 100"); + ImGui::PushItemWidth(-100); + ImGui::DragFloat("float##2a", &f); + if (show_indented_items) + { + ImGui::Indent(); + ImGui::DragFloat("float (indented)##2b", &f); + ImGui::Unindent(); + } + ImGui::PopItemWidth(); + + ImGui::Text("SetNextItemWidth/PushItemWidth(GetContentRegionAvail().x * 0.5f)"); + ImGui::SameLine(); HelpMarker("Half of available width.\n(~ right-cursor_pos)\n(works within a column set)"); + ImGui::PushItemWidth(ImGui::GetContentRegionAvail().x * 0.5f); + ImGui::DragFloat("float##3a", &f); + if (show_indented_items) + { + ImGui::Indent(); + ImGui::DragFloat("float (indented)##3b", &f); + ImGui::Unindent(); + } + ImGui::PopItemWidth(); + + ImGui::Text("SetNextItemWidth/PushItemWidth(-GetContentRegionAvail().x * 0.5f)"); + ImGui::SameLine(); HelpMarker("Align to right edge minus half"); + ImGui::PushItemWidth(-ImGui::GetContentRegionAvail().x * 0.5f); + ImGui::DragFloat("float##4a", &f); + if (show_indented_items) + { + ImGui::Indent(); + ImGui::DragFloat("float (indented)##4b", &f); + ImGui::Unindent(); + } + ImGui::PopItemWidth(); + + // Demonstrate using PushItemWidth to surround three items. + // Calling SetNextItemWidth() before each of them would have the same effect. + ImGui::Text("SetNextItemWidth/PushItemWidth(-FLT_MIN)"); + ImGui::SameLine(); HelpMarker("Align to right edge"); + ImGui::PushItemWidth(-FLT_MIN); + ImGui::DragFloat("##float5a", &f); + if (show_indented_items) + { + ImGui::Indent(); + ImGui::DragFloat("float (indented)##5b", &f); + ImGui::Unindent(); + } + ImGui::PopItemWidth(); + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Layout/Basic Horizontal Layout"); + if (ImGui::TreeNode("Basic Horizontal Layout")) + { + ImGui::TextWrapped("(Use ImGui::SameLine() to keep adding items to the right of the preceding item)"); + + // Text + IMGUI_DEMO_MARKER("Layout/Basic Horizontal Layout/SameLine"); + ImGui::Text("Two items: Hello"); ImGui::SameLine(); + ImGui::TextColored(ImVec4(1, 1, 0, 1), "Sailor"); + + // Adjust spacing + ImGui::Text("More spacing: Hello"); ImGui::SameLine(0, 20); + ImGui::TextColored(ImVec4(1, 1, 0, 1), "Sailor"); + + // Button + ImGui::AlignTextToFramePadding(); + ImGui::Text("Normal buttons"); ImGui::SameLine(); + ImGui::Button("Banana"); ImGui::SameLine(); + ImGui::Button("Apple"); ImGui::SameLine(); + ImGui::Button("Corniflower"); + + // Button + ImGui::Text("Small buttons"); ImGui::SameLine(); + ImGui::SmallButton("Like this one"); ImGui::SameLine(); + ImGui::Text("can fit within a text block."); + + // Aligned to arbitrary position. Easy/cheap column. + IMGUI_DEMO_MARKER("Layout/Basic Horizontal Layout/SameLine (with offset)"); + ImGui::Text("Aligned"); + ImGui::SameLine(150); ImGui::Text("x=150"); + ImGui::SameLine(300); ImGui::Text("x=300"); + ImGui::Text("Aligned"); + ImGui::SameLine(150); ImGui::SmallButton("x=150"); + ImGui::SameLine(300); ImGui::SmallButton("x=300"); + + // Checkbox + IMGUI_DEMO_MARKER("Layout/Basic Horizontal Layout/SameLine (more)"); + static bool c1 = false, c2 = false, c3 = false, c4 = false; + ImGui::Checkbox("My", &c1); ImGui::SameLine(); + ImGui::Checkbox("Tailor", &c2); ImGui::SameLine(); + ImGui::Checkbox("Is", &c3); ImGui::SameLine(); + ImGui::Checkbox("Rich", &c4); + + // Various + static float f0 = 1.0f, f1 = 2.0f, f2 = 3.0f; + ImGui::PushItemWidth(80); + const char* items[] = { "AAAA", "BBBB", "CCCC", "DDDD" }; + static int item = -1; + ImGui::Combo("Combo", &item, items, IM_ARRAYSIZE(items)); ImGui::SameLine(); + ImGui::SliderFloat("X", &f0, 0.0f, 5.0f); ImGui::SameLine(); + ImGui::SliderFloat("Y", &f1, 0.0f, 5.0f); ImGui::SameLine(); + ImGui::SliderFloat("Z", &f2, 0.0f, 5.0f); + ImGui::PopItemWidth(); + + ImGui::PushItemWidth(80); + ImGui::Text("Lists:"); + static int selection[4] = { 0, 1, 2, 3 }; + for (int i = 0; i < 4; i++) + { + if (i > 0) ImGui::SameLine(); + ImGui::PushID(i); + ImGui::ListBox("", &selection[i], items, IM_ARRAYSIZE(items)); + ImGui::PopID(); + //ImGui::SetItemTooltip("ListBox %d hovered", i); + } + ImGui::PopItemWidth(); + + // Dummy + IMGUI_DEMO_MARKER("Layout/Basic Horizontal Layout/Dummy"); + ImVec2 button_sz(40, 40); + ImGui::Button("A", button_sz); ImGui::SameLine(); + ImGui::Dummy(button_sz); ImGui::SameLine(); + ImGui::Button("B", button_sz); + + // Manually wrapping + // (we should eventually provide this as an automatic layout feature, but for now you can do it manually) + IMGUI_DEMO_MARKER("Layout/Basic Horizontal Layout/Manual wrapping"); + ImGui::Text("Manual wrapping:"); + ImGuiStyle& style = ImGui::GetStyle(); + int buttons_count = 20; + float window_visible_x2 = ImGui::GetWindowPos().x + ImGui::GetWindowContentRegionMax().x; + for (int n = 0; n < buttons_count; n++) + { + ImGui::PushID(n); + ImGui::Button("Box", button_sz); + float last_button_x2 = ImGui::GetItemRectMax().x; + float next_button_x2 = last_button_x2 + style.ItemSpacing.x + button_sz.x; // Expected position if next button was on same line + if (n + 1 < buttons_count && next_button_x2 < window_visible_x2) + ImGui::SameLine(); + ImGui::PopID(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Layout/Groups"); + if (ImGui::TreeNode("Groups")) + { + HelpMarker( + "BeginGroup() basically locks the horizontal position for new line. " + "EndGroup() bundles the whole group so that you can use \"item\" functions such as " + "IsItemHovered()/IsItemActive() or SameLine() etc. on the whole group."); + ImGui::BeginGroup(); + { + ImGui::BeginGroup(); + ImGui::Button("AAA"); + ImGui::SameLine(); + ImGui::Button("BBB"); + ImGui::SameLine(); + ImGui::BeginGroup(); + ImGui::Button("CCC"); + ImGui::Button("DDD"); + ImGui::EndGroup(); + ImGui::SameLine(); + ImGui::Button("EEE"); + ImGui::EndGroup(); + ImGui::SetItemTooltip("First group hovered"); + } + // Capture the group size and create widgets using the same size + ImVec2 size = ImGui::GetItemRectSize(); + const float values[5] = { 0.5f, 0.20f, 0.80f, 0.60f, 0.25f }; + ImGui::PlotHistogram("##values", values, IM_ARRAYSIZE(values), 0, NULL, 0.0f, 1.0f, size); + + ImGui::Button("ACTION", ImVec2((size.x - ImGui::GetStyle().ItemSpacing.x) * 0.5f, size.y)); + ImGui::SameLine(); + ImGui::Button("REACTION", ImVec2((size.x - ImGui::GetStyle().ItemSpacing.x) * 0.5f, size.y)); + ImGui::EndGroup(); + ImGui::SameLine(); + + ImGui::Button("LEVERAGE\nBUZZWORD", size); + ImGui::SameLine(); + + if (ImGui::BeginListBox("List", size)) + { + ImGui::Selectable("Selected", true); + ImGui::Selectable("Not Selected", false); + ImGui::EndListBox(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Layout/Text Baseline Alignment"); + if (ImGui::TreeNode("Text Baseline Alignment")) + { + { + ImGui::BulletText("Text baseline:"); + ImGui::SameLine(); HelpMarker( + "This is testing the vertical alignment that gets applied on text to keep it aligned with widgets. " + "Lines only composed of text or \"small\" widgets use less vertical space than lines with framed widgets."); + ImGui::Indent(); + + ImGui::Text("KO Blahblah"); ImGui::SameLine(); + ImGui::Button("Some framed item"); ImGui::SameLine(); + HelpMarker("Baseline of button will look misaligned with text.."); + + // If your line starts with text, call AlignTextToFramePadding() to align text to upcoming widgets. + // (because we don't know what's coming after the Text() statement, we need to move the text baseline + // down by FramePadding.y ahead of time) + ImGui::AlignTextToFramePadding(); + ImGui::Text("OK Blahblah"); ImGui::SameLine(); + ImGui::Button("Some framed item"); ImGui::SameLine(); + HelpMarker("We call AlignTextToFramePadding() to vertically align the text baseline by +FramePadding.y"); + + // SmallButton() uses the same vertical padding as Text + ImGui::Button("TEST##1"); ImGui::SameLine(); + ImGui::Text("TEST"); ImGui::SameLine(); + ImGui::SmallButton("TEST##2"); + + // If your line starts with text, call AlignTextToFramePadding() to align text to upcoming widgets. + ImGui::AlignTextToFramePadding(); + ImGui::Text("Text aligned to framed item"); ImGui::SameLine(); + ImGui::Button("Item##1"); ImGui::SameLine(); + ImGui::Text("Item"); ImGui::SameLine(); + ImGui::SmallButton("Item##2"); ImGui::SameLine(); + ImGui::Button("Item##3"); + + ImGui::Unindent(); + } + + ImGui::Spacing(); + + { + ImGui::BulletText("Multi-line text:"); + ImGui::Indent(); + ImGui::Text("One\nTwo\nThree"); ImGui::SameLine(); + ImGui::Text("Hello\nWorld"); ImGui::SameLine(); + ImGui::Text("Banana"); + + ImGui::Text("Banana"); ImGui::SameLine(); + ImGui::Text("Hello\nWorld"); ImGui::SameLine(); + ImGui::Text("One\nTwo\nThree"); + + ImGui::Button("HOP##1"); ImGui::SameLine(); + ImGui::Text("Banana"); ImGui::SameLine(); + ImGui::Text("Hello\nWorld"); ImGui::SameLine(); + ImGui::Text("Banana"); + + ImGui::Button("HOP##2"); ImGui::SameLine(); + ImGui::Text("Hello\nWorld"); ImGui::SameLine(); + ImGui::Text("Banana"); + ImGui::Unindent(); + } + + ImGui::Spacing(); + + { + ImGui::BulletText("Misc items:"); + ImGui::Indent(); + + // SmallButton() sets FramePadding to zero. Text baseline is aligned to match baseline of previous Button. + ImGui::Button("80x80", ImVec2(80, 80)); + ImGui::SameLine(); + ImGui::Button("50x50", ImVec2(50, 50)); + ImGui::SameLine(); + ImGui::Button("Button()"); + ImGui::SameLine(); + ImGui::SmallButton("SmallButton()"); + + // Tree + const float spacing = ImGui::GetStyle().ItemInnerSpacing.x; + ImGui::Button("Button##1"); + ImGui::SameLine(0.0f, spacing); + if (ImGui::TreeNode("Node##1")) + { + // Placeholder tree data + for (int i = 0; i < 6; i++) + ImGui::BulletText("Item %d..", i); + ImGui::TreePop(); + } + + // Vertically align text node a bit lower so it'll be vertically centered with upcoming widget. + // Otherwise you can use SmallButton() (smaller fit). + ImGui::AlignTextToFramePadding(); + + // Common mistake to avoid: if we want to SameLine after TreeNode we need to do it before we add + // other contents below the node. + bool node_open = ImGui::TreeNode("Node##2"); + ImGui::SameLine(0.0f, spacing); ImGui::Button("Button##2"); + if (node_open) + { + // Placeholder tree data + for (int i = 0; i < 6; i++) + ImGui::BulletText("Item %d..", i); + ImGui::TreePop(); + } + + // Bullet + ImGui::Button("Button##3"); + ImGui::SameLine(0.0f, spacing); + ImGui::BulletText("Bullet text"); + + ImGui::AlignTextToFramePadding(); + ImGui::BulletText("Node"); + ImGui::SameLine(0.0f, spacing); ImGui::Button("Button##4"); + ImGui::Unindent(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Layout/Scrolling"); + if (ImGui::TreeNode("Scrolling")) + { + // Vertical scroll functions + IMGUI_DEMO_MARKER("Layout/Scrolling/Vertical"); + HelpMarker("Use SetScrollHereY() or SetScrollFromPosY() to scroll to a given vertical position."); + + static int track_item = 50; + static bool enable_track = true; + static bool enable_extra_decorations = false; + static float scroll_to_off_px = 0.0f; + static float scroll_to_pos_px = 200.0f; + + ImGui::Checkbox("Decoration", &enable_extra_decorations); + + ImGui::Checkbox("Track", &enable_track); + ImGui::PushItemWidth(100); + ImGui::SameLine(140); enable_track |= ImGui::DragInt("##item", &track_item, 0.25f, 0, 99, "Item = %d"); + + bool scroll_to_off = ImGui::Button("Scroll Offset"); + ImGui::SameLine(140); scroll_to_off |= ImGui::DragFloat("##off", &scroll_to_off_px, 1.00f, 0, FLT_MAX, "+%.0f px"); + + bool scroll_to_pos = ImGui::Button("Scroll To Pos"); + ImGui::SameLine(140); scroll_to_pos |= ImGui::DragFloat("##pos", &scroll_to_pos_px, 1.00f, -10, FLT_MAX, "X/Y = %.0f px"); + ImGui::PopItemWidth(); + + if (scroll_to_off || scroll_to_pos) + enable_track = false; + + ImGuiStyle& style = ImGui::GetStyle(); + float child_w = (ImGui::GetContentRegionAvail().x - 4 * style.ItemSpacing.x) / 5; + if (child_w < 1.0f) + child_w = 1.0f; + ImGui::PushID("##VerticalScrolling"); + for (int i = 0; i < 5; i++) + { + if (i > 0) ImGui::SameLine(); + ImGui::BeginGroup(); + const char* names[] = { "Top", "25%", "Center", "75%", "Bottom" }; + ImGui::TextUnformatted(names[i]); + + const ImGuiWindowFlags child_flags = enable_extra_decorations ? ImGuiWindowFlags_MenuBar : 0; + const ImGuiID child_id = ImGui::GetID((void*)(intptr_t)i); + const bool child_is_visible = ImGui::BeginChild(child_id, ImVec2(child_w, 200.0f), true, child_flags); + if (ImGui::BeginMenuBar()) + { + ImGui::TextUnformatted("abc"); + ImGui::EndMenuBar(); + } + if (scroll_to_off) + ImGui::SetScrollY(scroll_to_off_px); + if (scroll_to_pos) + ImGui::SetScrollFromPosY(ImGui::GetCursorStartPos().y + scroll_to_pos_px, i * 0.25f); + if (child_is_visible) // Avoid calling SetScrollHereY when running with culled items + { + for (int item = 0; item < 100; item++) + { + if (enable_track && item == track_item) + { + ImGui::TextColored(ImVec4(1, 1, 0, 1), "Item %d", item); + ImGui::SetScrollHereY(i * 0.25f); // 0.0f:top, 0.5f:center, 1.0f:bottom + } + else + { + ImGui::Text("Item %d", item); + } + } + } + float scroll_y = ImGui::GetScrollY(); + float scroll_max_y = ImGui::GetScrollMaxY(); + ImGui::EndChild(); + ImGui::Text("%.0f/%.0f", scroll_y, scroll_max_y); + ImGui::EndGroup(); + } + ImGui::PopID(); + + // Horizontal scroll functions + IMGUI_DEMO_MARKER("Layout/Scrolling/Horizontal"); + ImGui::Spacing(); + HelpMarker( + "Use SetScrollHereX() or SetScrollFromPosX() to scroll to a given horizontal position.\n\n" + "Because the clipping rectangle of most window hides half worth of WindowPadding on the " + "left/right, using SetScrollFromPosX(+1) will usually result in clipped text whereas the " + "equivalent SetScrollFromPosY(+1) wouldn't."); + ImGui::PushID("##HorizontalScrolling"); + for (int i = 0; i < 5; i++) + { + float child_height = ImGui::GetTextLineHeight() + style.ScrollbarSize + style.WindowPadding.y * 2.0f; + ImGuiWindowFlags child_flags = ImGuiWindowFlags_HorizontalScrollbar | (enable_extra_decorations ? ImGuiWindowFlags_AlwaysVerticalScrollbar : 0); + ImGuiID child_id = ImGui::GetID((void*)(intptr_t)i); + bool child_is_visible = ImGui::BeginChild(child_id, ImVec2(-100, child_height), true, child_flags); + if (scroll_to_off) + ImGui::SetScrollX(scroll_to_off_px); + if (scroll_to_pos) + ImGui::SetScrollFromPosX(ImGui::GetCursorStartPos().x + scroll_to_pos_px, i * 0.25f); + if (child_is_visible) // Avoid calling SetScrollHereY when running with culled items + { + for (int item = 0; item < 100; item++) + { + if (item > 0) + ImGui::SameLine(); + if (enable_track && item == track_item) + { + ImGui::TextColored(ImVec4(1, 1, 0, 1), "Item %d", item); + ImGui::SetScrollHereX(i * 0.25f); // 0.0f:left, 0.5f:center, 1.0f:right + } + else + { + ImGui::Text("Item %d", item); + } + } + } + float scroll_x = ImGui::GetScrollX(); + float scroll_max_x = ImGui::GetScrollMaxX(); + ImGui::EndChild(); + ImGui::SameLine(); + const char* names[] = { "Left", "25%", "Center", "75%", "Right" }; + ImGui::Text("%s\n%.0f/%.0f", names[i], scroll_x, scroll_max_x); + ImGui::Spacing(); + } + ImGui::PopID(); + + // Miscellaneous Horizontal Scrolling Demo + IMGUI_DEMO_MARKER("Layout/Scrolling/Horizontal (more)"); + HelpMarker( + "Horizontal scrolling for a window is enabled via the ImGuiWindowFlags_HorizontalScrollbar flag.\n\n" + "You may want to also explicitly specify content width by using SetNextWindowContentWidth() before Begin()."); + static int lines = 7; + ImGui::SliderInt("Lines", &lines, 1, 15); + ImGui::PushStyleVar(ImGuiStyleVar_FrameRounding, 3.0f); + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(2.0f, 1.0f)); + ImVec2 scrolling_child_size = ImVec2(0, ImGui::GetFrameHeightWithSpacing() * 7 + 30); + ImGui::BeginChild("scrolling", scrolling_child_size, true, ImGuiWindowFlags_HorizontalScrollbar); + for (int line = 0; line < lines; line++) + { + // Display random stuff. For the sake of this trivial demo we are using basic Button() + SameLine() + // If you want to create your own time line for a real application you may be better off manipulating + // the cursor position yourself, aka using SetCursorPos/SetCursorScreenPos to position the widgets + // yourself. You may also want to use the lower-level ImDrawList API. + int num_buttons = 10 + ((line & 1) ? line * 9 : line * 3); + for (int n = 0; n < num_buttons; n++) + { + if (n > 0) ImGui::SameLine(); + ImGui::PushID(n + line * 1000); + char num_buf[16]; + sprintf(num_buf, "%d", n); + const char* label = (!(n % 15)) ? "FizzBuzz" : (!(n % 3)) ? "Fizz" : (!(n % 5)) ? "Buzz" : num_buf; + float hue = n * 0.05f; + ImGui::PushStyleColor(ImGuiCol_Button, (ImVec4)ImColor::HSV(hue, 0.6f, 0.6f)); + ImGui::PushStyleColor(ImGuiCol_ButtonHovered, (ImVec4)ImColor::HSV(hue, 0.7f, 0.7f)); + ImGui::PushStyleColor(ImGuiCol_ButtonActive, (ImVec4)ImColor::HSV(hue, 0.8f, 0.8f)); + ImGui::Button(label, ImVec2(40.0f + sinf((float)(line + n)) * 20.0f, 0.0f)); + ImGui::PopStyleColor(3); + ImGui::PopID(); + } + } + float scroll_x = ImGui::GetScrollX(); + float scroll_max_x = ImGui::GetScrollMaxX(); + ImGui::EndChild(); + ImGui::PopStyleVar(2); + float scroll_x_delta = 0.0f; + ImGui::SmallButton("<<"); + if (ImGui::IsItemActive()) + scroll_x_delta = -ImGui::GetIO().DeltaTime * 1000.0f; + ImGui::SameLine(); + ImGui::Text("Scroll from code"); ImGui::SameLine(); + ImGui::SmallButton(">>"); + if (ImGui::IsItemActive()) + scroll_x_delta = +ImGui::GetIO().DeltaTime * 1000.0f; + ImGui::SameLine(); + ImGui::Text("%.0f/%.0f", scroll_x, scroll_max_x); + if (scroll_x_delta != 0.0f) + { + // Demonstrate a trick: you can use Begin to set yourself in the context of another window + // (here we are already out of your child window) + ImGui::BeginChild("scrolling"); + ImGui::SetScrollX(ImGui::GetScrollX() + scroll_x_delta); + ImGui::EndChild(); + } + ImGui::Spacing(); + + static bool show_horizontal_contents_size_demo_window = false; + ImGui::Checkbox("Show Horizontal contents size demo window", &show_horizontal_contents_size_demo_window); + + if (show_horizontal_contents_size_demo_window) + { + static bool show_h_scrollbar = true; + static bool show_button = true; + static bool show_tree_nodes = true; + static bool show_text_wrapped = false; + static bool show_columns = true; + static bool show_tab_bar = true; + static bool show_child = false; + static bool explicit_content_size = false; + static float contents_size_x = 300.0f; + if (explicit_content_size) + ImGui::SetNextWindowContentSize(ImVec2(contents_size_x, 0.0f)); + ImGui::Begin("Horizontal contents size demo window", &show_horizontal_contents_size_demo_window, show_h_scrollbar ? ImGuiWindowFlags_HorizontalScrollbar : 0); + IMGUI_DEMO_MARKER("Layout/Scrolling/Horizontal contents size demo window"); + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(2, 0)); + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(2, 0)); + HelpMarker("Test of different widgets react and impact the work rectangle growing when horizontal scrolling is enabled.\n\nUse 'Metrics->Tools->Show windows rectangles' to visualize rectangles."); + ImGui::Checkbox("H-scrollbar", &show_h_scrollbar); + ImGui::Checkbox("Button", &show_button); // Will grow contents size (unless explicitly overwritten) + ImGui::Checkbox("Tree nodes", &show_tree_nodes); // Will grow contents size and display highlight over full width + ImGui::Checkbox("Text wrapped", &show_text_wrapped);// Will grow and use contents size + ImGui::Checkbox("Columns", &show_columns); // Will use contents size + ImGui::Checkbox("Tab bar", &show_tab_bar); // Will use contents size + ImGui::Checkbox("Child", &show_child); // Will grow and use contents size + ImGui::Checkbox("Explicit content size", &explicit_content_size); + ImGui::Text("Scroll %.1f/%.1f %.1f/%.1f", ImGui::GetScrollX(), ImGui::GetScrollMaxX(), ImGui::GetScrollY(), ImGui::GetScrollMaxY()); + if (explicit_content_size) + { + ImGui::SameLine(); + ImGui::SetNextItemWidth(100); + ImGui::DragFloat("##csx", &contents_size_x); + ImVec2 p = ImGui::GetCursorScreenPos(); + ImGui::GetWindowDrawList()->AddRectFilled(p, ImVec2(p.x + 10, p.y + 10), IM_COL32_WHITE); + ImGui::GetWindowDrawList()->AddRectFilled(ImVec2(p.x + contents_size_x - 10, p.y), ImVec2(p.x + contents_size_x, p.y + 10), IM_COL32_WHITE); + ImGui::Dummy(ImVec2(0, 10)); + } + ImGui::PopStyleVar(2); + ImGui::Separator(); + if (show_button) + { + ImGui::Button("this is a 300-wide button", ImVec2(300, 0)); + } + if (show_tree_nodes) + { + bool open = true; + if (ImGui::TreeNode("this is a tree node")) + { + if (ImGui::TreeNode("another one of those tree node...")) + { + ImGui::Text("Some tree contents"); + ImGui::TreePop(); + } + ImGui::TreePop(); + } + ImGui::CollapsingHeader("CollapsingHeader", &open); + } + if (show_text_wrapped) + { + ImGui::TextWrapped("This text should automatically wrap on the edge of the work rectangle."); + } + if (show_columns) + { + ImGui::Text("Tables:"); + if (ImGui::BeginTable("table", 4, ImGuiTableFlags_Borders)) + { + for (int n = 0; n < 4; n++) + { + ImGui::TableNextColumn(); + ImGui::Text("Width %.2f", ImGui::GetContentRegionAvail().x); + } + ImGui::EndTable(); + } + ImGui::Text("Columns:"); + ImGui::Columns(4); + for (int n = 0; n < 4; n++) + { + ImGui::Text("Width %.2f", ImGui::GetColumnWidth()); + ImGui::NextColumn(); + } + ImGui::Columns(1); + } + if (show_tab_bar && ImGui::BeginTabBar("Hello")) + { + if (ImGui::BeginTabItem("OneOneOne")) { ImGui::EndTabItem(); } + if (ImGui::BeginTabItem("TwoTwoTwo")) { ImGui::EndTabItem(); } + if (ImGui::BeginTabItem("ThreeThreeThree")) { ImGui::EndTabItem(); } + if (ImGui::BeginTabItem("FourFourFour")) { ImGui::EndTabItem(); } + ImGui::EndTabBar(); + } + if (show_child) + { + ImGui::BeginChild("child", ImVec2(0, 0), true); + ImGui::EndChild(); + } + ImGui::End(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Layout/Clipping"); + if (ImGui::TreeNode("Clipping")) + { + static ImVec2 size(100.0f, 100.0f); + static ImVec2 offset(30.0f, 30.0f); + ImGui::DragFloat2("size", (float*)&size, 0.5f, 1.0f, 200.0f, "%.0f"); + ImGui::TextWrapped("(Click and drag to scroll)"); + + HelpMarker( + "(Left) Using ImGui::PushClipRect():\n" + "Will alter ImGui hit-testing logic + ImDrawList rendering.\n" + "(use this if you want your clipping rectangle to affect interactions)\n\n" + "(Center) Using ImDrawList::PushClipRect():\n" + "Will alter ImDrawList rendering only.\n" + "(use this as a shortcut if you are only using ImDrawList calls)\n\n" + "(Right) Using ImDrawList::AddText() with a fine ClipRect:\n" + "Will alter only this specific ImDrawList::AddText() rendering.\n" + "This is often used internally to avoid altering the clipping rectangle and minimize draw calls."); + + for (int n = 0; n < 3; n++) + { + if (n > 0) + ImGui::SameLine(); + + ImGui::PushID(n); + ImGui::InvisibleButton("##canvas", size); + if (ImGui::IsItemActive() && ImGui::IsMouseDragging(ImGuiMouseButton_Left)) + { + offset.x += ImGui::GetIO().MouseDelta.x; + offset.y += ImGui::GetIO().MouseDelta.y; + } + ImGui::PopID(); + if (!ImGui::IsItemVisible()) // Skip rendering as ImDrawList elements are not clipped. + continue; + + const ImVec2 p0 = ImGui::GetItemRectMin(); + const ImVec2 p1 = ImGui::GetItemRectMax(); + const char* text_str = "Line 1 hello\nLine 2 clip me!"; + const ImVec2 text_pos = ImVec2(p0.x + offset.x, p0.y + offset.y); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + switch (n) + { + case 0: + ImGui::PushClipRect(p0, p1, true); + draw_list->AddRectFilled(p0, p1, IM_COL32(90, 90, 120, 255)); + draw_list->AddText(text_pos, IM_COL32_WHITE, text_str); + ImGui::PopClipRect(); + break; + case 1: + draw_list->PushClipRect(p0, p1, true); + draw_list->AddRectFilled(p0, p1, IM_COL32(90, 90, 120, 255)); + draw_list->AddText(text_pos, IM_COL32_WHITE, text_str); + draw_list->PopClipRect(); + break; + case 2: + ImVec4 clip_rect(p0.x, p0.y, p1.x, p1.y); // AddText() takes a ImVec4* here so let's convert. + draw_list->AddRectFilled(p0, p1, IM_COL32(90, 90, 120, 255)); + draw_list->AddText(ImGui::GetFont(), ImGui::GetFontSize(), text_pos, IM_COL32_WHITE, text_str, NULL, 0.0f, &clip_rect); + break; + } + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Layout/Overlap Mode"); + if (ImGui::TreeNode("Overlap Mode")) + { + static bool enable_allow_overlap = true; + + HelpMarker( + "Hit-testing is by default performed in item submission order, which generally is perceived as 'back-to-front'.\n\n" + "By using SetNextItemAllowOverlap() you can notify that an item may be overlapped by another. Doing so alters the hovering logic: items using AllowOverlap mode requires an extra frame to accept hovered state."); + ImGui::Checkbox("Enable AllowOverlap", &enable_allow_overlap); + + ImVec2 button1_pos = ImGui::GetCursorScreenPos(); + ImVec2 button2_pos = ImVec2(button1_pos.x + 50.0f, button1_pos.y + 50.0f); + if (enable_allow_overlap) + ImGui::SetNextItemAllowOverlap(); + ImGui::Button("Button 1", ImVec2(80, 80)); + ImGui::SetCursorScreenPos(button2_pos); + ImGui::Button("Button 2", ImVec2(80, 80)); + + // This is typically used with width-spanning items. + // (note that Selectable() has a dedicated flag ImGuiSelectableFlags_AllowOverlap, which is a shortcut + // for using SetNextItemAllowOverlap(). For demo purpose we use SetNextItemAllowOverlap() here.) + if (enable_allow_overlap) + ImGui::SetNextItemAllowOverlap(); + ImGui::Selectable("Some Selectable", false); + ImGui::SameLine(); + ImGui::SmallButton("++"); + + ImGui::TreePop(); + } +} + +static void ShowDemoWindowPopups() +{ + IMGUI_DEMO_MARKER("Popups"); + if (!ImGui::CollapsingHeader("Popups & Modal windows")) + return; + + // The properties of popups windows are: + // - They block normal mouse hovering detection outside them. (*) + // - Unless modal, they can be closed by clicking anywhere outside them, or by pressing ESCAPE. + // - Their visibility state (~bool) is held internally by Dear ImGui instead of being held by the programmer as + // we are used to with regular Begin() calls. User can manipulate the visibility state by calling OpenPopup(). + // (*) One can use IsItemHovered(ImGuiHoveredFlags_AllowWhenBlockedByPopup) to bypass it and detect hovering even + // when normally blocked by a popup. + // Those three properties are connected. The library needs to hold their visibility state BECAUSE it can close + // popups at any time. + + // Typical use for regular windows: + // bool my_tool_is_active = false; if (ImGui::Button("Open")) my_tool_is_active = true; [...] if (my_tool_is_active) Begin("My Tool", &my_tool_is_active) { [...] } End(); + // Typical use for popups: + // if (ImGui::Button("Open")) ImGui::OpenPopup("MyPopup"); if (ImGui::BeginPopup("MyPopup") { [...] EndPopup(); } + + // With popups we have to go through a library call (here OpenPopup) to manipulate the visibility state. + // This may be a bit confusing at first but it should quickly make sense. Follow on the examples below. + + IMGUI_DEMO_MARKER("Popups/Popups"); + if (ImGui::TreeNode("Popups")) + { + ImGui::TextWrapped( + "When a popup is active, it inhibits interacting with windows that are behind the popup. " + "Clicking outside the popup closes it."); + + static int selected_fish = -1; + const char* names[] = { "Bream", "Haddock", "Mackerel", "Pollock", "Tilefish" }; + static bool toggles[] = { true, false, false, false, false }; + + // Simple selection popup (if you want to show the current selection inside the Button itself, + // you may want to build a string using the "###" operator to preserve a constant ID with a variable label) + if (ImGui::Button("Select..")) + ImGui::OpenPopup("my_select_popup"); + ImGui::SameLine(); + ImGui::TextUnformatted(selected_fish == -1 ? "" : names[selected_fish]); + if (ImGui::BeginPopup("my_select_popup")) + { + ImGui::SeparatorText("Aquarium"); + for (int i = 0; i < IM_ARRAYSIZE(names); i++) + if (ImGui::Selectable(names[i])) + selected_fish = i; + ImGui::EndPopup(); + } + + // Showing a menu with toggles + if (ImGui::Button("Toggle..")) + ImGui::OpenPopup("my_toggle_popup"); + if (ImGui::BeginPopup("my_toggle_popup")) + { + for (int i = 0; i < IM_ARRAYSIZE(names); i++) + ImGui::MenuItem(names[i], "", &toggles[i]); + if (ImGui::BeginMenu("Sub-menu")) + { + ImGui::MenuItem("Click me"); + ImGui::EndMenu(); + } + + ImGui::Separator(); + ImGui::Text("Tooltip here"); + ImGui::SetItemTooltip("I am a tooltip over a popup"); + + if (ImGui::Button("Stacked Popup")) + ImGui::OpenPopup("another popup"); + if (ImGui::BeginPopup("another popup")) + { + for (int i = 0; i < IM_ARRAYSIZE(names); i++) + ImGui::MenuItem(names[i], "", &toggles[i]); + if (ImGui::BeginMenu("Sub-menu")) + { + ImGui::MenuItem("Click me"); + if (ImGui::Button("Stacked Popup")) + ImGui::OpenPopup("another popup"); + if (ImGui::BeginPopup("another popup")) + { + ImGui::Text("I am the last one here."); + ImGui::EndPopup(); + } + ImGui::EndMenu(); + } + ImGui::EndPopup(); + } + ImGui::EndPopup(); + } + + // Call the more complete ShowExampleMenuFile which we use in various places of this demo + if (ImGui::Button("With a menu..")) + ImGui::OpenPopup("my_file_popup"); + if (ImGui::BeginPopup("my_file_popup", ImGuiWindowFlags_MenuBar)) + { + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("File")) + { + ShowExampleMenuFile(); + ImGui::EndMenu(); + } + if (ImGui::BeginMenu("Edit")) + { + ImGui::MenuItem("Dummy"); + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + ImGui::Text("Hello from popup!"); + ImGui::Button("This is a dummy button.."); + ImGui::EndPopup(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Popups/Context menus"); + if (ImGui::TreeNode("Context menus")) + { + HelpMarker("\"Context\" functions are simple helpers to associate a Popup to a given Item or Window identifier."); + + // BeginPopupContextItem() is a helper to provide common/simple popup behavior of essentially doing: + // if (id == 0) + // id = GetItemID(); // Use last item id + // if (IsItemHovered() && IsMouseReleased(ImGuiMouseButton_Right)) + // OpenPopup(id); + // return BeginPopup(id); + // For advanced uses you may want to replicate and customize this code. + // See more details in BeginPopupContextItem(). + + // Example 1 + // When used after an item that has an ID (e.g. Button), we can skip providing an ID to BeginPopupContextItem(), + // and BeginPopupContextItem() will use the last item ID as the popup ID. + { + const char* names[5] = { "Label1", "Label2", "Label3", "Label4", "Label5" }; + static int selected = -1; + for (int n = 0; n < 5; n++) + { + if (ImGui::Selectable(names[n], selected == n)) + selected = n; + if (ImGui::BeginPopupContextItem()) // <-- use last item id as popup id + { + selected = n; + ImGui::Text("This a popup for \"%s\"!", names[n]); + if (ImGui::Button("Close")) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + ImGui::SetItemTooltip("Right-click to open popup"); + } + } + + // Example 2 + // Popup on a Text() element which doesn't have an identifier: we need to provide an identifier to BeginPopupContextItem(). + // Using an explicit identifier is also convenient if you want to activate the popups from different locations. + { + HelpMarker("Text() elements don't have stable identifiers so we need to provide one."); + static float value = 0.5f; + ImGui::Text("Value = %.3f <-- (1) right-click this text", value); + if (ImGui::BeginPopupContextItem("my popup")) + { + if (ImGui::Selectable("Set to zero")) value = 0.0f; + if (ImGui::Selectable("Set to PI")) value = 3.1415f; + ImGui::SetNextItemWidth(-FLT_MIN); + ImGui::DragFloat("##Value", &value, 0.1f, 0.0f, 0.0f); + ImGui::EndPopup(); + } + + // We can also use OpenPopupOnItemClick() to toggle the visibility of a given popup. + // Here we make it that right-clicking this other text element opens the same popup as above. + // The popup itself will be submitted by the code above. + ImGui::Text("(2) Or right-click this text"); + ImGui::OpenPopupOnItemClick("my popup", ImGuiPopupFlags_MouseButtonRight); + + // Back to square one: manually open the same popup. + if (ImGui::Button("(3) Or click this button")) + ImGui::OpenPopup("my popup"); + } + + // Example 3 + // When using BeginPopupContextItem() with an implicit identifier (NULL == use last item ID), + // we need to make sure your item identifier is stable. + // In this example we showcase altering the item label while preserving its identifier, using the ### operator (see FAQ). + { + HelpMarker("Showcase using a popup ID linked to item ID, with the item having a changing label + stable ID using the ### operator."); + static char name[32] = "Label1"; + char buf[64]; + sprintf(buf, "Button: %s###Button", name); // ### operator override ID ignoring the preceding label + ImGui::Button(buf); + if (ImGui::BeginPopupContextItem()) + { + ImGui::Text("Edit name:"); + ImGui::InputText("##edit", name, IM_ARRAYSIZE(name)); + if (ImGui::Button("Close")) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + ImGui::SameLine(); ImGui::Text("(<-- right-click here)"); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Popups/Modals"); + if (ImGui::TreeNode("Modals")) + { + ImGui::TextWrapped("Modal windows are like popups but the user cannot close them by clicking outside."); + + if (ImGui::Button("Delete..")) + ImGui::OpenPopup("Delete?"); + + // Always center this window when appearing + ImVec2 center = ImGui::GetMainViewport()->GetCenter(); + ImGui::SetNextWindowPos(center, ImGuiCond_Appearing, ImVec2(0.5f, 0.5f)); + + if (ImGui::BeginPopupModal("Delete?", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("All those beautiful files will be deleted.\nThis operation cannot be undone!"); + ImGui::Separator(); + + //static int unused_i = 0; + //ImGui::Combo("Combo", &unused_i, "Delete\0Delete harder\0"); + + static bool dont_ask_me_next_time = false; + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(0, 0)); + ImGui::Checkbox("Don't ask me next time", &dont_ask_me_next_time); + ImGui::PopStyleVar(); + + if (ImGui::Button("OK", ImVec2(120, 0))) { ImGui::CloseCurrentPopup(); } + ImGui::SetItemDefaultFocus(); + ImGui::SameLine(); + if (ImGui::Button("Cancel", ImVec2(120, 0))) { ImGui::CloseCurrentPopup(); } + ImGui::EndPopup(); + } + + if (ImGui::Button("Stacked modals..")) + ImGui::OpenPopup("Stacked 1"); + if (ImGui::BeginPopupModal("Stacked 1", NULL, ImGuiWindowFlags_MenuBar)) + { + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("File")) + { + if (ImGui::MenuItem("Some menu item")) {} + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + ImGui::Text("Hello from Stacked The First\nUsing style.Colors[ImGuiCol_ModalWindowDimBg] behind it."); + + // Testing behavior of widgets stacking their own regular popups over the modal. + static int item = 1; + static float color[4] = { 0.4f, 0.7f, 0.0f, 0.5f }; + ImGui::Combo("Combo", &item, "aaaa\0bbbb\0cccc\0dddd\0eeee\0\0"); + ImGui::ColorEdit4("color", color); + + if (ImGui::Button("Add another modal..")) + ImGui::OpenPopup("Stacked 2"); + + // Also demonstrate passing a bool* to BeginPopupModal(), this will create a regular close button which + // will close the popup. Note that the visibility state of popups is owned by imgui, so the input value + // of the bool actually doesn't matter here. + bool unused_open = true; + if (ImGui::BeginPopupModal("Stacked 2", &unused_open)) + { + ImGui::Text("Hello from Stacked The Second!"); + if (ImGui::Button("Close")) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + + if (ImGui::Button("Close")) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Popups/Menus inside a regular window"); + if (ImGui::TreeNode("Menus inside a regular window")) + { + ImGui::TextWrapped("Below we are testing adding menu items to a regular window. It's rather unusual but should work!"); + ImGui::Separator(); + + ImGui::MenuItem("Menu item", "CTRL+M"); + if (ImGui::BeginMenu("Menu inside a regular window")) + { + ShowExampleMenuFile(); + ImGui::EndMenu(); + } + ImGui::Separator(); + ImGui::TreePop(); + } +} + +// Dummy data structure that we use for the Table demo. +// (pre-C++11 doesn't allow us to instantiate ImVector template if this structure is defined inside the demo function) +namespace +{ +// We are passing our own identifier to TableSetupColumn() to facilitate identifying columns in the sorting code. +// This identifier will be passed down into ImGuiTableSortSpec::ColumnUserID. +// But it is possible to omit the user id parameter of TableSetupColumn() and just use the column index instead! (ImGuiTableSortSpec::ColumnIndex) +// If you don't use sorting, you will generally never care about giving column an ID! +enum MyItemColumnID +{ + MyItemColumnID_ID, + MyItemColumnID_Name, + MyItemColumnID_Action, + MyItemColumnID_Quantity, + MyItemColumnID_Description +}; + +struct MyItem +{ + int ID; + const char* Name; + int Quantity; + + // We have a problem which is affecting _only this demo_ and should not affect your code: + // As we don't rely on std:: or other third-party library to compile dear imgui, we only have reliable access to qsort(), + // however qsort doesn't allow passing user data to comparing function. + // As a workaround, we are storing the sort specs in a static/global for the comparing function to access. + // In your own use case you would probably pass the sort specs to your sorting/comparing functions directly and not use a global. + // We could technically call ImGui::TableGetSortSpecs() in CompareWithSortSpecs(), but considering that this function is called + // very often by the sorting algorithm it would be a little wasteful. + static const ImGuiTableSortSpecs* s_current_sort_specs; + + static void SortWithSortSpecs(ImGuiTableSortSpecs* sort_specs, MyItem* items, int items_count) + { + s_current_sort_specs = sort_specs; // Store in variable accessible by the sort function. + if (items_count > 1) + qsort(items, (size_t)items_count, sizeof(items[0]), MyItem::CompareWithSortSpecs); + s_current_sort_specs = NULL; + } + + // Compare function to be used by qsort() + static int IMGUI_CDECL CompareWithSortSpecs(const void* lhs, const void* rhs) + { + const MyItem* a = (const MyItem*)lhs; + const MyItem* b = (const MyItem*)rhs; + for (int n = 0; n < s_current_sort_specs->SpecsCount; n++) + { + // Here we identify columns using the ColumnUserID value that we ourselves passed to TableSetupColumn() + // We could also choose to identify columns based on their index (sort_spec->ColumnIndex), which is simpler! + const ImGuiTableColumnSortSpecs* sort_spec = &s_current_sort_specs->Specs[n]; + int delta = 0; + switch (sort_spec->ColumnUserID) + { + case MyItemColumnID_ID: delta = (a->ID - b->ID); break; + case MyItemColumnID_Name: delta = (strcmp(a->Name, b->Name)); break; + case MyItemColumnID_Quantity: delta = (a->Quantity - b->Quantity); break; + case MyItemColumnID_Description: delta = (strcmp(a->Name, b->Name)); break; + default: IM_ASSERT(0); break; + } + if (delta > 0) + return (sort_spec->SortDirection == ImGuiSortDirection_Ascending) ? +1 : -1; + if (delta < 0) + return (sort_spec->SortDirection == ImGuiSortDirection_Ascending) ? -1 : +1; + } + + // qsort() is instable so always return a way to differenciate items. + // Your own compare function may want to avoid fallback on implicit sort specs e.g. a Name compare if it wasn't already part of the sort specs. + return (a->ID - b->ID); + } +}; +const ImGuiTableSortSpecs* MyItem::s_current_sort_specs = NULL; +} + +// Make the UI compact because there are so many fields +static void PushStyleCompact() +{ + ImGuiStyle& style = ImGui::GetStyle(); + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(style.FramePadding.x, (float)(int)(style.FramePadding.y * 0.60f))); + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(style.ItemSpacing.x, (float)(int)(style.ItemSpacing.y * 0.60f))); +} + +static void PopStyleCompact() +{ + ImGui::PopStyleVar(2); +} + +// Show a combo box with a choice of sizing policies +static void EditTableSizingFlags(ImGuiTableFlags* p_flags) +{ + struct EnumDesc { ImGuiTableFlags Value; const char* Name; const char* Tooltip; }; + static const EnumDesc policies[] = + { + { ImGuiTableFlags_None, "Default", "Use default sizing policy:\n- ImGuiTableFlags_SizingFixedFit if ScrollX is on or if host window has ImGuiWindowFlags_AlwaysAutoResize.\n- ImGuiTableFlags_SizingStretchSame otherwise." }, + { ImGuiTableFlags_SizingFixedFit, "ImGuiTableFlags_SizingFixedFit", "Columns default to _WidthFixed (if resizable) or _WidthAuto (if not resizable), matching contents width." }, + { ImGuiTableFlags_SizingFixedSame, "ImGuiTableFlags_SizingFixedSame", "Columns are all the same width, matching the maximum contents width.\nImplicitly disable ImGuiTableFlags_Resizable and enable ImGuiTableFlags_NoKeepColumnsVisible." }, + { ImGuiTableFlags_SizingStretchProp, "ImGuiTableFlags_SizingStretchProp", "Columns default to _WidthStretch with weights proportional to their widths." }, + { ImGuiTableFlags_SizingStretchSame, "ImGuiTableFlags_SizingStretchSame", "Columns default to _WidthStretch with same weights." } + }; + int idx; + for (idx = 0; idx < IM_ARRAYSIZE(policies); idx++) + if (policies[idx].Value == (*p_flags & ImGuiTableFlags_SizingMask_)) + break; + const char* preview_text = (idx < IM_ARRAYSIZE(policies)) ? policies[idx].Name + (idx > 0 ? strlen("ImGuiTableFlags") : 0) : ""; + if (ImGui::BeginCombo("Sizing Policy", preview_text)) + { + for (int n = 0; n < IM_ARRAYSIZE(policies); n++) + if (ImGui::Selectable(policies[n].Name, idx == n)) + *p_flags = (*p_flags & ~ImGuiTableFlags_SizingMask_) | policies[n].Value; + ImGui::EndCombo(); + } + ImGui::SameLine(); + ImGui::TextDisabled("(?)"); + if (ImGui::BeginItemTooltip()) + { + ImGui::PushTextWrapPos(ImGui::GetFontSize() * 50.0f); + for (int m = 0; m < IM_ARRAYSIZE(policies); m++) + { + ImGui::Separator(); + ImGui::Text("%s:", policies[m].Name); + ImGui::Separator(); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + ImGui::GetStyle().IndentSpacing * 0.5f); + ImGui::TextUnformatted(policies[m].Tooltip); + } + ImGui::PopTextWrapPos(); + ImGui::EndTooltip(); + } +} + +static void EditTableColumnsFlags(ImGuiTableColumnFlags* p_flags) +{ + ImGui::CheckboxFlags("_Disabled", p_flags, ImGuiTableColumnFlags_Disabled); ImGui::SameLine(); HelpMarker("Master disable flag (also hide from context menu)"); + ImGui::CheckboxFlags("_DefaultHide", p_flags, ImGuiTableColumnFlags_DefaultHide); + ImGui::CheckboxFlags("_DefaultSort", p_flags, ImGuiTableColumnFlags_DefaultSort); + if (ImGui::CheckboxFlags("_WidthStretch", p_flags, ImGuiTableColumnFlags_WidthStretch)) + *p_flags &= ~(ImGuiTableColumnFlags_WidthMask_ ^ ImGuiTableColumnFlags_WidthStretch); + if (ImGui::CheckboxFlags("_WidthFixed", p_flags, ImGuiTableColumnFlags_WidthFixed)) + *p_flags &= ~(ImGuiTableColumnFlags_WidthMask_ ^ ImGuiTableColumnFlags_WidthFixed); + ImGui::CheckboxFlags("_NoResize", p_flags, ImGuiTableColumnFlags_NoResize); + ImGui::CheckboxFlags("_NoReorder", p_flags, ImGuiTableColumnFlags_NoReorder); + ImGui::CheckboxFlags("_NoHide", p_flags, ImGuiTableColumnFlags_NoHide); + ImGui::CheckboxFlags("_NoClip", p_flags, ImGuiTableColumnFlags_NoClip); + ImGui::CheckboxFlags("_NoSort", p_flags, ImGuiTableColumnFlags_NoSort); + ImGui::CheckboxFlags("_NoSortAscending", p_flags, ImGuiTableColumnFlags_NoSortAscending); + ImGui::CheckboxFlags("_NoSortDescending", p_flags, ImGuiTableColumnFlags_NoSortDescending); + ImGui::CheckboxFlags("_NoHeaderLabel", p_flags, ImGuiTableColumnFlags_NoHeaderLabel); + ImGui::CheckboxFlags("_NoHeaderWidth", p_flags, ImGuiTableColumnFlags_NoHeaderWidth); + ImGui::CheckboxFlags("_PreferSortAscending", p_flags, ImGuiTableColumnFlags_PreferSortAscending); + ImGui::CheckboxFlags("_PreferSortDescending", p_flags, ImGuiTableColumnFlags_PreferSortDescending); + ImGui::CheckboxFlags("_IndentEnable", p_flags, ImGuiTableColumnFlags_IndentEnable); ImGui::SameLine(); HelpMarker("Default for column 0"); + ImGui::CheckboxFlags("_IndentDisable", p_flags, ImGuiTableColumnFlags_IndentDisable); ImGui::SameLine(); HelpMarker("Default for column >0"); +} + +static void ShowTableColumnsStatusFlags(ImGuiTableColumnFlags flags) +{ + ImGui::CheckboxFlags("_IsEnabled", &flags, ImGuiTableColumnFlags_IsEnabled); + ImGui::CheckboxFlags("_IsVisible", &flags, ImGuiTableColumnFlags_IsVisible); + ImGui::CheckboxFlags("_IsSorted", &flags, ImGuiTableColumnFlags_IsSorted); + ImGui::CheckboxFlags("_IsHovered", &flags, ImGuiTableColumnFlags_IsHovered); +} + +static void ShowDemoWindowTables() +{ + //ImGui::SetNextItemOpen(true, ImGuiCond_Once); + IMGUI_DEMO_MARKER("Tables"); + if (!ImGui::CollapsingHeader("Tables & Columns")) + return; + + // Using those as a base value to create width/height that are factor of the size of our font + const float TEXT_BASE_WIDTH = ImGui::CalcTextSize("A").x; + const float TEXT_BASE_HEIGHT = ImGui::GetTextLineHeightWithSpacing(); + + ImGui::PushID("Tables"); + + int open_action = -1; + if (ImGui::Button("Open all")) + open_action = 1; + ImGui::SameLine(); + if (ImGui::Button("Close all")) + open_action = 0; + ImGui::SameLine(); + + // Options + static bool disable_indent = false; + ImGui::Checkbox("Disable tree indentation", &disable_indent); + ImGui::SameLine(); + HelpMarker("Disable the indenting of tree nodes so demo tables can use the full window width."); + ImGui::Separator(); + if (disable_indent) + ImGui::PushStyleVar(ImGuiStyleVar_IndentSpacing, 0.0f); + + // About Styling of tables + // Most settings are configured on a per-table basis via the flags passed to BeginTable() and TableSetupColumns APIs. + // There are however a few settings that a shared and part of the ImGuiStyle structure: + // style.CellPadding // Padding within each cell + // style.Colors[ImGuiCol_TableHeaderBg] // Table header background + // style.Colors[ImGuiCol_TableBorderStrong] // Table outer and header borders + // style.Colors[ImGuiCol_TableBorderLight] // Table inner borders + // style.Colors[ImGuiCol_TableRowBg] // Table row background when ImGuiTableFlags_RowBg is enabled (even rows) + // style.Colors[ImGuiCol_TableRowBgAlt] // Table row background when ImGuiTableFlags_RowBg is enabled (odds rows) + + // Demos + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Basic"); + if (ImGui::TreeNode("Basic")) + { + // Here we will showcase three different ways to output a table. + // They are very simple variations of a same thing! + + // [Method 1] Using TableNextRow() to create a new row, and TableSetColumnIndex() to select the column. + // In many situations, this is the most flexible and easy to use pattern. + HelpMarker("Using TableNextRow() + calling TableSetColumnIndex() _before_ each cell, in a loop."); + if (ImGui::BeginTable("table1", 3)) + { + for (int row = 0; row < 4; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("Row %d Column %d", row, column); + } + } + ImGui::EndTable(); + } + + // [Method 2] Using TableNextColumn() called multiple times, instead of using a for loop + TableSetColumnIndex(). + // This is generally more convenient when you have code manually submitting the contents of each column. + HelpMarker("Using TableNextRow() + calling TableNextColumn() _before_ each cell, manually."); + if (ImGui::BeginTable("table2", 3)) + { + for (int row = 0; row < 4; row++) + { + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + ImGui::Text("Row %d", row); + ImGui::TableNextColumn(); + ImGui::Text("Some contents"); + ImGui::TableNextColumn(); + ImGui::Text("123.456"); + } + ImGui::EndTable(); + } + + // [Method 3] We call TableNextColumn() _before_ each cell. We never call TableNextRow(), + // as TableNextColumn() will automatically wrap around and create new rows as needed. + // This is generally more convenient when your cells all contains the same type of data. + HelpMarker( + "Only using TableNextColumn(), which tends to be convenient for tables where every cell contains the same type of contents.\n" + "This is also more similar to the old NextColumn() function of the Columns API, and provided to facilitate the Columns->Tables API transition."); + if (ImGui::BeginTable("table3", 3)) + { + for (int item = 0; item < 14; item++) + { + ImGui::TableNextColumn(); + ImGui::Text("Item %d", item); + } + ImGui::EndTable(); + } + + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Borders, background"); + if (ImGui::TreeNode("Borders, background")) + { + // Expose a few Borders related flags interactively + enum ContentsType { CT_Text, CT_FillButton }; + static ImGuiTableFlags flags = ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg; + static bool display_headers = false; + static int contents_type = CT_Text; + + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_RowBg", &flags, ImGuiTableFlags_RowBg); + ImGui::CheckboxFlags("ImGuiTableFlags_Borders", &flags, ImGuiTableFlags_Borders); + ImGui::SameLine(); HelpMarker("ImGuiTableFlags_Borders\n = ImGuiTableFlags_BordersInnerV\n | ImGuiTableFlags_BordersOuterV\n | ImGuiTableFlags_BordersInnerV\n | ImGuiTableFlags_BordersOuterH"); + ImGui::Indent(); + + ImGui::CheckboxFlags("ImGuiTableFlags_BordersH", &flags, ImGuiTableFlags_BordersH); + ImGui::Indent(); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuterH", &flags, ImGuiTableFlags_BordersOuterH); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInnerH", &flags, ImGuiTableFlags_BordersInnerH); + ImGui::Unindent(); + + ImGui::CheckboxFlags("ImGuiTableFlags_BordersV", &flags, ImGuiTableFlags_BordersV); + ImGui::Indent(); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuterV", &flags, ImGuiTableFlags_BordersOuterV); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInnerV", &flags, ImGuiTableFlags_BordersInnerV); + ImGui::Unindent(); + + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuter", &flags, ImGuiTableFlags_BordersOuter); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInner", &flags, ImGuiTableFlags_BordersInner); + ImGui::Unindent(); + + ImGui::AlignTextToFramePadding(); ImGui::Text("Cell contents:"); + ImGui::SameLine(); ImGui::RadioButton("Text", &contents_type, CT_Text); + ImGui::SameLine(); ImGui::RadioButton("FillButton", &contents_type, CT_FillButton); + ImGui::Checkbox("Display headers", &display_headers); + ImGui::CheckboxFlags("ImGuiTableFlags_NoBordersInBody", &flags, ImGuiTableFlags_NoBordersInBody); ImGui::SameLine(); HelpMarker("Disable vertical borders in columns Body (borders will always appear in Headers"); + PopStyleCompact(); + + if (ImGui::BeginTable("table1", 3, flags)) + { + // Display headers so we can inspect their interaction with borders. + // (Headers are not the main purpose of this section of the demo, so we are not elaborating on them too much. See other sections for details) + if (display_headers) + { + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + ImGui::TableHeadersRow(); + } + + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + char buf[32]; + sprintf(buf, "Hello %d,%d", column, row); + if (contents_type == CT_Text) + ImGui::TextUnformatted(buf); + else if (contents_type == CT_FillButton) + ImGui::Button(buf, ImVec2(-FLT_MIN, 0.0f)); + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Resizable, stretch"); + if (ImGui::TreeNode("Resizable, stretch")) + { + // By default, if we don't enable ScrollX the sizing policy for each column is "Stretch" + // All columns maintain a sizing weight, and they will occupy all available width. + static ImGuiTableFlags flags = ImGuiTableFlags_SizingStretchSame | ImGuiTableFlags_Resizable | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_BordersV | ImGuiTableFlags_ContextMenuInBody; + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags, ImGuiTableFlags_Resizable); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersV", &flags, ImGuiTableFlags_BordersV); + ImGui::SameLine(); HelpMarker("Using the _Resizable flag automatically enables the _BordersInnerV flag as well, this is why the resize borders are still showing when unchecking this."); + PopStyleCompact(); + + if (ImGui::BeginTable("table1", 3, flags)) + { + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("Hello %d,%d", column, row); + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Resizable, fixed"); + if (ImGui::TreeNode("Resizable, fixed")) + { + // Here we use ImGuiTableFlags_SizingFixedFit (even though _ScrollX is not set) + // So columns will adopt the "Fixed" policy and will maintain a fixed width regardless of the whole available width (unless table is small) + // If there is not enough available width to fit all columns, they will however be resized down. + // FIXME-TABLE: Providing a stretch-on-init would make sense especially for tables which don't have saved settings + HelpMarker( + "Using _Resizable + _SizingFixedFit flags.\n" + "Fixed-width columns generally makes more sense if you want to use horizontal scrolling.\n\n" + "Double-click a column border to auto-fit the column to its contents."); + PushStyleCompact(); + static ImGuiTableFlags flags = ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_Resizable | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_BordersV | ImGuiTableFlags_ContextMenuInBody; + ImGui::CheckboxFlags("ImGuiTableFlags_NoHostExtendX", &flags, ImGuiTableFlags_NoHostExtendX); + PopStyleCompact(); + + if (ImGui::BeginTable("table1", 3, flags)) + { + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("Hello %d,%d", column, row); + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Resizable, mixed"); + if (ImGui::TreeNode("Resizable, mixed")) + { + HelpMarker( + "Using TableSetupColumn() to alter resizing policy on a per-column basis.\n\n" + "When combining Fixed and Stretch columns, generally you only want one, maybe two trailing columns to use _WidthStretch."); + static ImGuiTableFlags flags = ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_RowBg | ImGuiTableFlags_Borders | ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable; + + if (ImGui::BeginTable("table1", 3, flags)) + { + ImGui::TableSetupColumn("AAA", ImGuiTableColumnFlags_WidthFixed); + ImGui::TableSetupColumn("BBB", ImGuiTableColumnFlags_WidthFixed); + ImGui::TableSetupColumn("CCC", ImGuiTableColumnFlags_WidthStretch); + ImGui::TableHeadersRow(); + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("%s %d,%d", (column == 2) ? "Stretch" : "Fixed", column, row); + } + } + ImGui::EndTable(); + } + if (ImGui::BeginTable("table2", 6, flags)) + { + ImGui::TableSetupColumn("AAA", ImGuiTableColumnFlags_WidthFixed); + ImGui::TableSetupColumn("BBB", ImGuiTableColumnFlags_WidthFixed); + ImGui::TableSetupColumn("CCC", ImGuiTableColumnFlags_WidthFixed | ImGuiTableColumnFlags_DefaultHide); + ImGui::TableSetupColumn("DDD", ImGuiTableColumnFlags_WidthStretch); + ImGui::TableSetupColumn("EEE", ImGuiTableColumnFlags_WidthStretch); + ImGui::TableSetupColumn("FFF", ImGuiTableColumnFlags_WidthStretch | ImGuiTableColumnFlags_DefaultHide); + ImGui::TableHeadersRow(); + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 6; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("%s %d,%d", (column >= 3) ? "Stretch" : "Fixed", column, row); + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Reorderable, hideable, with headers"); + if (ImGui::TreeNode("Reorderable, hideable, with headers")) + { + HelpMarker( + "Click and drag column headers to reorder columns.\n\n" + "Right-click on a header to open a context menu."); + static ImGuiTableFlags flags = ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_BordersV; + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags, ImGuiTableFlags_Resizable); + ImGui::CheckboxFlags("ImGuiTableFlags_Reorderable", &flags, ImGuiTableFlags_Reorderable); + ImGui::CheckboxFlags("ImGuiTableFlags_Hideable", &flags, ImGuiTableFlags_Hideable); + ImGui::CheckboxFlags("ImGuiTableFlags_NoBordersInBody", &flags, ImGuiTableFlags_NoBordersInBody); + ImGui::CheckboxFlags("ImGuiTableFlags_NoBordersInBodyUntilResize", &flags, ImGuiTableFlags_NoBordersInBodyUntilResize); ImGui::SameLine(); HelpMarker("Disable vertical borders in columns Body until hovered for resize (borders will always appear in Headers)"); + PopStyleCompact(); + + if (ImGui::BeginTable("table1", 3, flags)) + { + // Submit columns name with TableSetupColumn() and call TableHeadersRow() to create a row with a header in each column. + // (Later we will show how TableSetupColumn() has other uses, optional flags, sizing weight etc.) + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + ImGui::TableHeadersRow(); + for (int row = 0; row < 6; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("Hello %d,%d", column, row); + } + } + ImGui::EndTable(); + } + + // Use outer_size.x == 0.0f instead of default to make the table as tight as possible (only valid when no scrolling and no stretch column) + if (ImGui::BeginTable("table2", 3, flags | ImGuiTableFlags_SizingFixedFit, ImVec2(0.0f, 0.0f))) + { + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + ImGui::TableHeadersRow(); + for (int row = 0; row < 6; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("Fixed %d,%d", column, row); + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Padding"); + if (ImGui::TreeNode("Padding")) + { + // First example: showcase use of padding flags and effect of BorderOuterV/BorderInnerV on X padding. + // We don't expose BorderOuterH/BorderInnerH here because they have no effect on X padding. + HelpMarker( + "We often want outer padding activated when any using features which makes the edges of a column visible:\n" + "e.g.:\n" + "- BorderOuterV\n" + "- any form of row selection\n" + "Because of this, activating BorderOuterV sets the default to PadOuterX. Using PadOuterX or NoPadOuterX you can override the default.\n\n" + "Actual padding values are using style.CellPadding.\n\n" + "In this demo we don't show horizontal borders to emphasize how they don't affect default horizontal padding."); + + static ImGuiTableFlags flags1 = ImGuiTableFlags_BordersV; + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_PadOuterX", &flags1, ImGuiTableFlags_PadOuterX); + ImGui::SameLine(); HelpMarker("Enable outer-most padding (default if ImGuiTableFlags_BordersOuterV is set)"); + ImGui::CheckboxFlags("ImGuiTableFlags_NoPadOuterX", &flags1, ImGuiTableFlags_NoPadOuterX); + ImGui::SameLine(); HelpMarker("Disable outer-most padding (default if ImGuiTableFlags_BordersOuterV is not set)"); + ImGui::CheckboxFlags("ImGuiTableFlags_NoPadInnerX", &flags1, ImGuiTableFlags_NoPadInnerX); + ImGui::SameLine(); HelpMarker("Disable inner padding between columns (double inner padding if BordersOuterV is on, single inner padding if BordersOuterV is off)"); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuterV", &flags1, ImGuiTableFlags_BordersOuterV); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInnerV", &flags1, ImGuiTableFlags_BordersInnerV); + static bool show_headers = false; + ImGui::Checkbox("show_headers", &show_headers); + PopStyleCompact(); + + if (ImGui::BeginTable("table_padding", 3, flags1)) + { + if (show_headers) + { + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + ImGui::TableHeadersRow(); + } + + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + if (row == 0) + { + ImGui::Text("Avail %.2f", ImGui::GetContentRegionAvail().x); + } + else + { + char buf[32]; + sprintf(buf, "Hello %d,%d", column, row); + ImGui::Button(buf, ImVec2(-FLT_MIN, 0.0f)); + } + //if (ImGui::TableGetColumnFlags() & ImGuiTableColumnFlags_IsHovered) + // ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, IM_COL32(0, 100, 0, 255)); + } + } + ImGui::EndTable(); + } + + // Second example: set style.CellPadding to (0.0) or a custom value. + // FIXME-TABLE: Vertical border effectively not displayed the same way as horizontal one... + HelpMarker("Setting style.CellPadding to (0,0) or a custom value."); + static ImGuiTableFlags flags2 = ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg; + static ImVec2 cell_padding(0.0f, 0.0f); + static bool show_widget_frame_bg = true; + + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_Borders", &flags2, ImGuiTableFlags_Borders); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersH", &flags2, ImGuiTableFlags_BordersH); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersV", &flags2, ImGuiTableFlags_BordersV); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInner", &flags2, ImGuiTableFlags_BordersInner); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuter", &flags2, ImGuiTableFlags_BordersOuter); + ImGui::CheckboxFlags("ImGuiTableFlags_RowBg", &flags2, ImGuiTableFlags_RowBg); + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags2, ImGuiTableFlags_Resizable); + ImGui::Checkbox("show_widget_frame_bg", &show_widget_frame_bg); + ImGui::SliderFloat2("CellPadding", &cell_padding.x, 0.0f, 10.0f, "%.0f"); + PopStyleCompact(); + + ImGui::PushStyleVar(ImGuiStyleVar_CellPadding, cell_padding); + if (ImGui::BeginTable("table_padding_2", 3, flags2)) + { + static char text_bufs[3 * 5][16]; // Mini text storage for 3x5 cells + static bool init = true; + if (!show_widget_frame_bg) + ImGui::PushStyleColor(ImGuiCol_FrameBg, 0); + for (int cell = 0; cell < 3 * 5; cell++) + { + ImGui::TableNextColumn(); + if (init) + strcpy(text_bufs[cell], "edit me"); + ImGui::SetNextItemWidth(-FLT_MIN); + ImGui::PushID(cell); + ImGui::InputText("##cell", text_bufs[cell], IM_ARRAYSIZE(text_bufs[cell])); + ImGui::PopID(); + } + if (!show_widget_frame_bg) + ImGui::PopStyleColor(); + init = false; + ImGui::EndTable(); + } + ImGui::PopStyleVar(); + + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Explicit widths"); + if (ImGui::TreeNode("Sizing policies")) + { + static ImGuiTableFlags flags1 = ImGuiTableFlags_BordersV | ImGuiTableFlags_BordersOuterH | ImGuiTableFlags_RowBg | ImGuiTableFlags_ContextMenuInBody; + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags1, ImGuiTableFlags_Resizable); + ImGui::CheckboxFlags("ImGuiTableFlags_NoHostExtendX", &flags1, ImGuiTableFlags_NoHostExtendX); + PopStyleCompact(); + + static ImGuiTableFlags sizing_policy_flags[4] = { ImGuiTableFlags_SizingFixedFit, ImGuiTableFlags_SizingFixedSame, ImGuiTableFlags_SizingStretchProp, ImGuiTableFlags_SizingStretchSame }; + for (int table_n = 0; table_n < 4; table_n++) + { + ImGui::PushID(table_n); + ImGui::SetNextItemWidth(TEXT_BASE_WIDTH * 30); + EditTableSizingFlags(&sizing_policy_flags[table_n]); + + // To make it easier to understand the different sizing policy, + // For each policy: we display one table where the columns have equal contents width, and one where the columns have different contents width. + if (ImGui::BeginTable("table1", 3, sizing_policy_flags[table_n] | flags1)) + { + for (int row = 0; row < 3; row++) + { + ImGui::TableNextRow(); + ImGui::TableNextColumn(); ImGui::Text("Oh dear"); + ImGui::TableNextColumn(); ImGui::Text("Oh dear"); + ImGui::TableNextColumn(); ImGui::Text("Oh dear"); + } + ImGui::EndTable(); + } + if (ImGui::BeginTable("table2", 3, sizing_policy_flags[table_n] | flags1)) + { + for (int row = 0; row < 3; row++) + { + ImGui::TableNextRow(); + ImGui::TableNextColumn(); ImGui::Text("AAAA"); + ImGui::TableNextColumn(); ImGui::Text("BBBBBBBB"); + ImGui::TableNextColumn(); ImGui::Text("CCCCCCCCCCCC"); + } + ImGui::EndTable(); + } + ImGui::PopID(); + } + + ImGui::Spacing(); + ImGui::TextUnformatted("Advanced"); + ImGui::SameLine(); + HelpMarker("This section allows you to interact and see the effect of various sizing policies depending on whether Scroll is enabled and the contents of your columns."); + + enum ContentsType { CT_ShowWidth, CT_ShortText, CT_LongText, CT_Button, CT_FillButton, CT_InputText }; + static ImGuiTableFlags flags = ImGuiTableFlags_ScrollY | ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg | ImGuiTableFlags_Resizable; + static int contents_type = CT_ShowWidth; + static int column_count = 3; + + PushStyleCompact(); + ImGui::PushID("Advanced"); + ImGui::PushItemWidth(TEXT_BASE_WIDTH * 30); + EditTableSizingFlags(&flags); + ImGui::Combo("Contents", &contents_type, "Show width\0Short Text\0Long Text\0Button\0Fill Button\0InputText\0"); + if (contents_type == CT_FillButton) + { + ImGui::SameLine(); + HelpMarker("Be mindful that using right-alignment (e.g. size.x = -FLT_MIN) creates a feedback loop where contents width can feed into auto-column width can feed into contents width."); + } + ImGui::DragInt("Columns", &column_count, 0.1f, 1, 64, "%d", ImGuiSliderFlags_AlwaysClamp); + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags, ImGuiTableFlags_Resizable); + ImGui::CheckboxFlags("ImGuiTableFlags_PreciseWidths", &flags, ImGuiTableFlags_PreciseWidths); + ImGui::SameLine(); HelpMarker("Disable distributing remainder width to stretched columns (width allocation on a 100-wide table with 3 columns: Without this flag: 33,33,34. With this flag: 33,33,33). With larger number of columns, resizing will appear to be less smooth."); + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollX", &flags, ImGuiTableFlags_ScrollX); + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollY", &flags, ImGuiTableFlags_ScrollY); + ImGui::CheckboxFlags("ImGuiTableFlags_NoClip", &flags, ImGuiTableFlags_NoClip); + ImGui::PopItemWidth(); + ImGui::PopID(); + PopStyleCompact(); + + if (ImGui::BeginTable("table2", column_count, flags, ImVec2(0.0f, TEXT_BASE_HEIGHT * 7))) + { + for (int cell = 0; cell < 10 * column_count; cell++) + { + ImGui::TableNextColumn(); + int column = ImGui::TableGetColumnIndex(); + int row = ImGui::TableGetRowIndex(); + + ImGui::PushID(cell); + char label[32]; + static char text_buf[32] = ""; + sprintf(label, "Hello %d,%d", column, row); + switch (contents_type) + { + case CT_ShortText: ImGui::TextUnformatted(label); break; + case CT_LongText: ImGui::Text("Some %s text %d,%d\nOver two lines..", column == 0 ? "long" : "longeeer", column, row); break; + case CT_ShowWidth: ImGui::Text("W: %.1f", ImGui::GetContentRegionAvail().x); break; + case CT_Button: ImGui::Button(label); break; + case CT_FillButton: ImGui::Button(label, ImVec2(-FLT_MIN, 0.0f)); break; + case CT_InputText: ImGui::SetNextItemWidth(-FLT_MIN); ImGui::InputText("##", text_buf, IM_ARRAYSIZE(text_buf)); break; + } + ImGui::PopID(); + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Vertical scrolling, with clipping"); + if (ImGui::TreeNode("Vertical scrolling, with clipping")) + { + HelpMarker("Here we activate ScrollY, which will create a child window container to allow hosting scrollable contents.\n\nWe also demonstrate using ImGuiListClipper to virtualize the submission of many items."); + static ImGuiTableFlags flags = ImGuiTableFlags_ScrollY | ImGuiTableFlags_RowBg | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_BordersV | ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable; + + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollY", &flags, ImGuiTableFlags_ScrollY); + PopStyleCompact(); + + // When using ScrollX or ScrollY we need to specify a size for our table container! + // Otherwise by default the table will fit all available space, like a BeginChild() call. + ImVec2 outer_size = ImVec2(0.0f, TEXT_BASE_HEIGHT * 8); + if (ImGui::BeginTable("table_scrolly", 3, flags, outer_size)) + { + ImGui::TableSetupScrollFreeze(0, 1); // Make top row always visible + ImGui::TableSetupColumn("One", ImGuiTableColumnFlags_None); + ImGui::TableSetupColumn("Two", ImGuiTableColumnFlags_None); + ImGui::TableSetupColumn("Three", ImGuiTableColumnFlags_None); + ImGui::TableHeadersRow(); + + // Demonstrate using clipper for large vertical lists + ImGuiListClipper clipper; + clipper.Begin(1000); + while (clipper.Step()) + { + for (int row = clipper.DisplayStart; row < clipper.DisplayEnd; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("Hello %d,%d", column, row); + } + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Horizontal scrolling"); + if (ImGui::TreeNode("Horizontal scrolling")) + { + HelpMarker( + "When ScrollX is enabled, the default sizing policy becomes ImGuiTableFlags_SizingFixedFit, " + "as automatically stretching columns doesn't make much sense with horizontal scrolling.\n\n" + "Also note that as of the current version, you will almost always want to enable ScrollY along with ScrollX," + "because the container window won't automatically extend vertically to fix contents (this may be improved in future versions)."); + static ImGuiTableFlags flags = ImGuiTableFlags_ScrollX | ImGuiTableFlags_ScrollY | ImGuiTableFlags_RowBg | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_BordersV | ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable; + static int freeze_cols = 1; + static int freeze_rows = 1; + + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags, ImGuiTableFlags_Resizable); + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollX", &flags, ImGuiTableFlags_ScrollX); + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollY", &flags, ImGuiTableFlags_ScrollY); + ImGui::SetNextItemWidth(ImGui::GetFrameHeight()); + ImGui::DragInt("freeze_cols", &freeze_cols, 0.2f, 0, 9, NULL, ImGuiSliderFlags_NoInput); + ImGui::SetNextItemWidth(ImGui::GetFrameHeight()); + ImGui::DragInt("freeze_rows", &freeze_rows, 0.2f, 0, 9, NULL, ImGuiSliderFlags_NoInput); + PopStyleCompact(); + + // When using ScrollX or ScrollY we need to specify a size for our table container! + // Otherwise by default the table will fit all available space, like a BeginChild() call. + ImVec2 outer_size = ImVec2(0.0f, TEXT_BASE_HEIGHT * 8); + if (ImGui::BeginTable("table_scrollx", 7, flags, outer_size)) + { + ImGui::TableSetupScrollFreeze(freeze_cols, freeze_rows); + ImGui::TableSetupColumn("Line #", ImGuiTableColumnFlags_NoHide); // Make the first column not hideable to match our use of TableSetupScrollFreeze() + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + ImGui::TableSetupColumn("Four"); + ImGui::TableSetupColumn("Five"); + ImGui::TableSetupColumn("Six"); + ImGui::TableHeadersRow(); + for (int row = 0; row < 20; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 7; column++) + { + // Both TableNextColumn() and TableSetColumnIndex() return true when a column is visible or performing width measurement. + // Because here we know that: + // - A) all our columns are contributing the same to row height + // - B) column 0 is always visible, + // We only always submit this one column and can skip others. + // More advanced per-column clipping behaviors may benefit from polling the status flags via TableGetColumnFlags(). + if (!ImGui::TableSetColumnIndex(column) && column > 0) + continue; + if (column == 0) + ImGui::Text("Line %d", row); + else + ImGui::Text("Hello world %d,%d", column, row); + } + } + ImGui::EndTable(); + } + + ImGui::Spacing(); + ImGui::TextUnformatted("Stretch + ScrollX"); + ImGui::SameLine(); + HelpMarker( + "Showcase using Stretch columns + ScrollX together: " + "this is rather unusual and only makes sense when specifying an 'inner_width' for the table!\n" + "Without an explicit value, inner_width is == outer_size.x and therefore using Stretch columns + ScrollX together doesn't make sense."); + static ImGuiTableFlags flags2 = ImGuiTableFlags_SizingStretchSame | ImGuiTableFlags_ScrollX | ImGuiTableFlags_ScrollY | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_RowBg | ImGuiTableFlags_ContextMenuInBody; + static float inner_width = 1000.0f; + PushStyleCompact(); + ImGui::PushID("flags3"); + ImGui::PushItemWidth(TEXT_BASE_WIDTH * 30); + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollX", &flags2, ImGuiTableFlags_ScrollX); + ImGui::DragFloat("inner_width", &inner_width, 1.0f, 0.0f, FLT_MAX, "%.1f"); + ImGui::PopItemWidth(); + ImGui::PopID(); + PopStyleCompact(); + if (ImGui::BeginTable("table2", 7, flags2, outer_size, inner_width)) + { + for (int cell = 0; cell < 20 * 7; cell++) + { + ImGui::TableNextColumn(); + ImGui::Text("Hello world %d,%d", ImGui::TableGetColumnIndex(), ImGui::TableGetRowIndex()); + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Columns flags"); + if (ImGui::TreeNode("Columns flags")) + { + // Create a first table just to show all the options/flags we want to make visible in our example! + const int column_count = 3; + const char* column_names[column_count] = { "One", "Two", "Three" }; + static ImGuiTableColumnFlags column_flags[column_count] = { ImGuiTableColumnFlags_DefaultSort, ImGuiTableColumnFlags_None, ImGuiTableColumnFlags_DefaultHide }; + static ImGuiTableColumnFlags column_flags_out[column_count] = { 0, 0, 0 }; // Output from TableGetColumnFlags() + + if (ImGui::BeginTable("table_columns_flags_checkboxes", column_count, ImGuiTableFlags_None)) + { + PushStyleCompact(); + for (int column = 0; column < column_count; column++) + { + ImGui::TableNextColumn(); + ImGui::PushID(column); + ImGui::AlignTextToFramePadding(); // FIXME-TABLE: Workaround for wrong text baseline propagation across columns + ImGui::Text("'%s'", column_names[column]); + ImGui::Spacing(); + ImGui::Text("Input flags:"); + EditTableColumnsFlags(&column_flags[column]); + ImGui::Spacing(); + ImGui::Text("Output flags:"); + ImGui::BeginDisabled(); + ShowTableColumnsStatusFlags(column_flags_out[column]); + ImGui::EndDisabled(); + ImGui::PopID(); + } + PopStyleCompact(); + ImGui::EndTable(); + } + + // Create the real table we care about for the example! + // We use a scrolling table to be able to showcase the difference between the _IsEnabled and _IsVisible flags above, otherwise in + // a non-scrolling table columns are always visible (unless using ImGuiTableFlags_NoKeepColumnsVisible + resizing the parent window down) + const ImGuiTableFlags flags + = ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_ScrollX | ImGuiTableFlags_ScrollY + | ImGuiTableFlags_RowBg | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_BordersV + | ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable | ImGuiTableFlags_Sortable; + ImVec2 outer_size = ImVec2(0.0f, TEXT_BASE_HEIGHT * 9); + if (ImGui::BeginTable("table_columns_flags", column_count, flags, outer_size)) + { + for (int column = 0; column < column_count; column++) + ImGui::TableSetupColumn(column_names[column], column_flags[column]); + ImGui::TableHeadersRow(); + for (int column = 0; column < column_count; column++) + column_flags_out[column] = ImGui::TableGetColumnFlags(column); + float indent_step = (float)((int)TEXT_BASE_WIDTH / 2); + for (int row = 0; row < 8; row++) + { + ImGui::Indent(indent_step); // Add some indentation to demonstrate usage of per-column IndentEnable/IndentDisable flags. + ImGui::TableNextRow(); + for (int column = 0; column < column_count; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("%s %s", (column == 0) ? "Indented" : "Hello", ImGui::TableGetColumnName(column)); + } + } + ImGui::Unindent(indent_step * 8.0f); + + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Columns widths"); + if (ImGui::TreeNode("Columns widths")) + { + HelpMarker("Using TableSetupColumn() to setup default width."); + + static ImGuiTableFlags flags1 = ImGuiTableFlags_Borders | ImGuiTableFlags_NoBordersInBodyUntilResize; + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags1, ImGuiTableFlags_Resizable); + ImGui::CheckboxFlags("ImGuiTableFlags_NoBordersInBodyUntilResize", &flags1, ImGuiTableFlags_NoBordersInBodyUntilResize); + PopStyleCompact(); + if (ImGui::BeginTable("table1", 3, flags1)) + { + // We could also set ImGuiTableFlags_SizingFixedFit on the table and all columns will default to ImGuiTableColumnFlags_WidthFixed. + ImGui::TableSetupColumn("one", ImGuiTableColumnFlags_WidthFixed, 100.0f); // Default to 100.0f + ImGui::TableSetupColumn("two", ImGuiTableColumnFlags_WidthFixed, 200.0f); // Default to 200.0f + ImGui::TableSetupColumn("three", ImGuiTableColumnFlags_WidthFixed); // Default to auto + ImGui::TableHeadersRow(); + for (int row = 0; row < 4; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableSetColumnIndex(column); + if (row == 0) + ImGui::Text("(w: %5.1f)", ImGui::GetContentRegionAvail().x); + else + ImGui::Text("Hello %d,%d", column, row); + } + } + ImGui::EndTable(); + } + + HelpMarker("Using TableSetupColumn() to setup explicit width.\n\nUnless _NoKeepColumnsVisible is set, fixed columns with set width may still be shrunk down if there's not enough space in the host."); + + static ImGuiTableFlags flags2 = ImGuiTableFlags_None; + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_NoKeepColumnsVisible", &flags2, ImGuiTableFlags_NoKeepColumnsVisible); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInnerV", &flags2, ImGuiTableFlags_BordersInnerV); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuterV", &flags2, ImGuiTableFlags_BordersOuterV); + PopStyleCompact(); + if (ImGui::BeginTable("table2", 4, flags2)) + { + // We could also set ImGuiTableFlags_SizingFixedFit on the table and all columns will default to ImGuiTableColumnFlags_WidthFixed. + ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed, 100.0f); + ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed, TEXT_BASE_WIDTH * 15.0f); + ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed, TEXT_BASE_WIDTH * 30.0f); + ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed, TEXT_BASE_WIDTH * 15.0f); + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 4; column++) + { + ImGui::TableSetColumnIndex(column); + if (row == 0) + ImGui::Text("(w: %5.1f)", ImGui::GetContentRegionAvail().x); + else + ImGui::Text("Hello %d,%d", column, row); + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Nested tables"); + if (ImGui::TreeNode("Nested tables")) + { + HelpMarker("This demonstrates embedding a table into another table cell."); + + if (ImGui::BeginTable("table_nested1", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable)) + { + ImGui::TableSetupColumn("A0"); + ImGui::TableSetupColumn("A1"); + ImGui::TableHeadersRow(); + + ImGui::TableNextColumn(); + ImGui::Text("A0 Row 0"); + { + float rows_height = TEXT_BASE_HEIGHT * 2; + if (ImGui::BeginTable("table_nested2", 2, ImGuiTableFlags_Borders | ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable)) + { + ImGui::TableSetupColumn("B0"); + ImGui::TableSetupColumn("B1"); + ImGui::TableHeadersRow(); + + ImGui::TableNextRow(ImGuiTableRowFlags_None, rows_height); + ImGui::TableNextColumn(); + ImGui::Text("B0 Row 0"); + ImGui::TableNextColumn(); + ImGui::Text("B1 Row 0"); + ImGui::TableNextRow(ImGuiTableRowFlags_None, rows_height); + ImGui::TableNextColumn(); + ImGui::Text("B0 Row 1"); + ImGui::TableNextColumn(); + ImGui::Text("B1 Row 1"); + + ImGui::EndTable(); + } + } + ImGui::TableNextColumn(); ImGui::Text("A1 Row 0"); + ImGui::TableNextColumn(); ImGui::Text("A0 Row 1"); + ImGui::TableNextColumn(); ImGui::Text("A1 Row 1"); + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Row height"); + if (ImGui::TreeNode("Row height")) + { + HelpMarker("You can pass a 'min_row_height' to TableNextRow().\n\nRows are padded with 'style.CellPadding.y' on top and bottom, so effectively the minimum row height will always be >= 'style.CellPadding.y * 2.0f'.\n\nWe cannot honor a _maximum_ row height as that would require a unique clipping rectangle per row."); + if (ImGui::BeginTable("table_row_height", 1, ImGuiTableFlags_Borders)) + { + for (int row = 0; row < 8; row++) + { + float min_row_height = (float)(int)(TEXT_BASE_HEIGHT * 0.30f * row); + ImGui::TableNextRow(ImGuiTableRowFlags_None, min_row_height); + ImGui::TableNextColumn(); + ImGui::Text("min_row_height = %.2f", min_row_height); + } + ImGui::EndTable(); + } + + HelpMarker("Showcase using SameLine(0,0) to share Current Line Height between cells.\n\nPlease note that Tables Row Height is not the same thing as Current Line Height, as a table cell may contains multiple lines."); + if (ImGui::BeginTable("table_share_lineheight", 2, ImGuiTableFlags_Borders)) + { + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + ImGui::ColorButton("##1", ImVec4(0.13f, 0.26f, 0.40f, 1.0f), ImGuiColorEditFlags_None, ImVec2(40, 40)); + ImGui::TableNextColumn(); + ImGui::Text("Line 1"); + ImGui::Text("Line 2"); + + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + ImGui::ColorButton("##2", ImVec4(0.13f, 0.26f, 0.40f, 1.0f), ImGuiColorEditFlags_None, ImVec2(40, 40)); + ImGui::TableNextColumn(); + ImGui::SameLine(0.0f, 0.0f); // Reuse line height from previous column + ImGui::Text("Line 1, with SameLine(0,0)"); + ImGui::Text("Line 2"); + + ImGui::EndTable(); + } + + HelpMarker("Showcase altering CellPadding.y between rows. Note that CellPadding.x is locked for the entire table."); + if (ImGui::BeginTable("table_changing_cellpadding_y", 1, ImGuiTableFlags_Borders)) + { + ImGuiStyle& style = ImGui::GetStyle(); + for (int row = 0; row < 8; row++) + { + if ((row % 3) == 2) + ImGui::PushStyleVar(ImGuiStyleVar_CellPadding, ImVec2(style.CellPadding.x, 20.0f)); + ImGui::TableNextRow(ImGuiTableRowFlags_None); + ImGui::TableNextColumn(); + ImGui::Text("CellPadding.y = %.2f", style.CellPadding.y); + if ((row % 3) == 2) + ImGui::PopStyleVar(); + } + ImGui::EndTable(); + } + + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Outer size"); + if (ImGui::TreeNode("Outer size")) + { + // Showcasing use of ImGuiTableFlags_NoHostExtendX and ImGuiTableFlags_NoHostExtendY + // Important to that note how the two flags have slightly different behaviors! + ImGui::Text("Using NoHostExtendX and NoHostExtendY:"); + PushStyleCompact(); + static ImGuiTableFlags flags = ImGuiTableFlags_Borders | ImGuiTableFlags_Resizable | ImGuiTableFlags_ContextMenuInBody | ImGuiTableFlags_RowBg | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoHostExtendX; + ImGui::CheckboxFlags("ImGuiTableFlags_NoHostExtendX", &flags, ImGuiTableFlags_NoHostExtendX); + ImGui::SameLine(); HelpMarker("Make outer width auto-fit to columns, overriding outer_size.x value.\n\nOnly available when ScrollX/ScrollY are disabled and Stretch columns are not used."); + ImGui::CheckboxFlags("ImGuiTableFlags_NoHostExtendY", &flags, ImGuiTableFlags_NoHostExtendY); + ImGui::SameLine(); HelpMarker("Make outer height stop exactly at outer_size.y (prevent auto-extending table past the limit).\n\nOnly available when ScrollX/ScrollY are disabled. Data below the limit will be clipped and not visible."); + PopStyleCompact(); + + ImVec2 outer_size = ImVec2(0.0f, TEXT_BASE_HEIGHT * 5.5f); + if (ImGui::BeginTable("table1", 3, flags, outer_size)) + { + for (int row = 0; row < 10; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableNextColumn(); + ImGui::Text("Cell %d,%d", column, row); + } + } + ImGui::EndTable(); + } + ImGui::SameLine(); + ImGui::Text("Hello!"); + + ImGui::Spacing(); + + ImGui::Text("Using explicit size:"); + if (ImGui::BeginTable("table2", 3, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg, ImVec2(TEXT_BASE_WIDTH * 30, 0.0f))) + { + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + ImGui::TableNextColumn(); + ImGui::Text("Cell %d,%d", column, row); + } + } + ImGui::EndTable(); + } + ImGui::SameLine(); + if (ImGui::BeginTable("table3", 3, ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg, ImVec2(TEXT_BASE_WIDTH * 30, 0.0f))) + { + for (int row = 0; row < 3; row++) + { + ImGui::TableNextRow(0, TEXT_BASE_HEIGHT * 1.5f); + for (int column = 0; column < 3; column++) + { + ImGui::TableNextColumn(); + ImGui::Text("Cell %d,%d", column, row); + } + } + ImGui::EndTable(); + } + + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Background color"); + if (ImGui::TreeNode("Background color")) + { + static ImGuiTableFlags flags = ImGuiTableFlags_RowBg; + static int row_bg_type = 1; + static int row_bg_target = 1; + static int cell_bg_type = 1; + + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_Borders", &flags, ImGuiTableFlags_Borders); + ImGui::CheckboxFlags("ImGuiTableFlags_RowBg", &flags, ImGuiTableFlags_RowBg); + ImGui::SameLine(); HelpMarker("ImGuiTableFlags_RowBg automatically sets RowBg0 to alternative colors pulled from the Style."); + ImGui::Combo("row bg type", (int*)&row_bg_type, "None\0Red\0Gradient\0"); + ImGui::Combo("row bg target", (int*)&row_bg_target, "RowBg0\0RowBg1\0"); ImGui::SameLine(); HelpMarker("Target RowBg0 to override the alternating odd/even colors,\nTarget RowBg1 to blend with them."); + ImGui::Combo("cell bg type", (int*)&cell_bg_type, "None\0Blue\0"); ImGui::SameLine(); HelpMarker("We are colorizing cells to B1->C2 here."); + IM_ASSERT(row_bg_type >= 0 && row_bg_type <= 2); + IM_ASSERT(row_bg_target >= 0 && row_bg_target <= 1); + IM_ASSERT(cell_bg_type >= 0 && cell_bg_type <= 1); + PopStyleCompact(); + + if (ImGui::BeginTable("table1", 5, flags)) + { + for (int row = 0; row < 6; row++) + { + ImGui::TableNextRow(); + + // Demonstrate setting a row background color with 'ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBgX, ...)' + // We use a transparent color so we can see the one behind in case our target is RowBg1 and RowBg0 was already targeted by the ImGuiTableFlags_RowBg flag. + if (row_bg_type != 0) + { + ImU32 row_bg_color = ImGui::GetColorU32(row_bg_type == 1 ? ImVec4(0.7f, 0.3f, 0.3f, 0.65f) : ImVec4(0.2f + row * 0.1f, 0.2f, 0.2f, 0.65f)); // Flat or Gradient? + ImGui::TableSetBgColor(ImGuiTableBgTarget_RowBg0 + row_bg_target, row_bg_color); + } + + // Fill cells + for (int column = 0; column < 5; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("%c%c", 'A' + row, '0' + column); + + // Change background of Cells B1->C2 + // Demonstrate setting a cell background color with 'ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, ...)' + // (the CellBg color will be blended over the RowBg and ColumnBg colors) + // We can also pass a column number as a third parameter to TableSetBgColor() and do this outside the column loop. + if (row >= 1 && row <= 2 && column >= 1 && column <= 2 && cell_bg_type == 1) + { + ImU32 cell_bg_color = ImGui::GetColorU32(ImVec4(0.3f, 0.3f, 0.7f, 0.65f)); + ImGui::TableSetBgColor(ImGuiTableBgTarget_CellBg, cell_bg_color); + } + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Tree view"); + if (ImGui::TreeNode("Tree view")) + { + static ImGuiTableFlags flags = ImGuiTableFlags_BordersV | ImGuiTableFlags_BordersOuterH | ImGuiTableFlags_Resizable | ImGuiTableFlags_RowBg | ImGuiTableFlags_NoBordersInBody; + + if (ImGui::BeginTable("3ways", 3, flags)) + { + // The first column will use the default _WidthStretch when ScrollX is Off and _WidthFixed when ScrollX is On + ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_NoHide); + ImGui::TableSetupColumn("Size", ImGuiTableColumnFlags_WidthFixed, TEXT_BASE_WIDTH * 12.0f); + ImGui::TableSetupColumn("Type", ImGuiTableColumnFlags_WidthFixed, TEXT_BASE_WIDTH * 18.0f); + ImGui::TableHeadersRow(); + + // Simple storage to output a dummy file-system. + struct MyTreeNode + { + const char* Name; + const char* Type; + int Size; + int ChildIdx; + int ChildCount; + static void DisplayNode(const MyTreeNode* node, const MyTreeNode* all_nodes) + { + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + const bool is_folder = (node->ChildCount > 0); + if (is_folder) + { + bool open = ImGui::TreeNodeEx(node->Name, ImGuiTreeNodeFlags_SpanFullWidth); + ImGui::TableNextColumn(); + ImGui::TextDisabled("--"); + ImGui::TableNextColumn(); + ImGui::TextUnformatted(node->Type); + if (open) + { + for (int child_n = 0; child_n < node->ChildCount; child_n++) + DisplayNode(&all_nodes[node->ChildIdx + child_n], all_nodes); + ImGui::TreePop(); + } + } + else + { + ImGui::TreeNodeEx(node->Name, ImGuiTreeNodeFlags_Leaf | ImGuiTreeNodeFlags_Bullet | ImGuiTreeNodeFlags_NoTreePushOnOpen | ImGuiTreeNodeFlags_SpanFullWidth); + ImGui::TableNextColumn(); + ImGui::Text("%d", node->Size); + ImGui::TableNextColumn(); + ImGui::TextUnformatted(node->Type); + } + } + }; + static const MyTreeNode nodes[] = + { + { "Root", "Folder", -1, 1, 3 }, // 0 + { "Music", "Folder", -1, 4, 2 }, // 1 + { "Textures", "Folder", -1, 6, 3 }, // 2 + { "desktop.ini", "System file", 1024, -1,-1 }, // 3 + { "File1_a.wav", "Audio file", 123000, -1,-1 }, // 4 + { "File1_b.wav", "Audio file", 456000, -1,-1 }, // 5 + { "Image001.png", "Image file", 203128, -1,-1 }, // 6 + { "Copy of Image001.png", "Image file", 203256, -1,-1 }, // 7 + { "Copy of Image001 (Final2).png","Image file", 203512, -1,-1 }, // 8 + }; + + MyTreeNode::DisplayNode(&nodes[0], nodes); + + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Item width"); + if (ImGui::TreeNode("Item width")) + { + HelpMarker( + "Showcase using PushItemWidth() and how it is preserved on a per-column basis.\n\n" + "Note that on auto-resizing non-resizable fixed columns, querying the content width for e.g. right-alignment doesn't make sense."); + if (ImGui::BeginTable("table_item_width", 3, ImGuiTableFlags_Borders)) + { + ImGui::TableSetupColumn("small"); + ImGui::TableSetupColumn("half"); + ImGui::TableSetupColumn("right-align"); + ImGui::TableHeadersRow(); + + for (int row = 0; row < 3; row++) + { + ImGui::TableNextRow(); + if (row == 0) + { + // Setup ItemWidth once (instead of setting up every time, which is also possible but less efficient) + ImGui::TableSetColumnIndex(0); + ImGui::PushItemWidth(TEXT_BASE_WIDTH * 3.0f); // Small + ImGui::TableSetColumnIndex(1); + ImGui::PushItemWidth(-ImGui::GetContentRegionAvail().x * 0.5f); + ImGui::TableSetColumnIndex(2); + ImGui::PushItemWidth(-FLT_MIN); // Right-aligned + } + + // Draw our contents + static float dummy_f = 0.0f; + ImGui::PushID(row); + ImGui::TableSetColumnIndex(0); + ImGui::SliderFloat("float0", &dummy_f, 0.0f, 1.0f); + ImGui::TableSetColumnIndex(1); + ImGui::SliderFloat("float1", &dummy_f, 0.0f, 1.0f); + ImGui::TableSetColumnIndex(2); + ImGui::SliderFloat("##float2", &dummy_f, 0.0f, 1.0f); // No visible label since right-aligned + ImGui::PopID(); + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + // Demonstrate using TableHeader() calls instead of TableHeadersRow() + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Custom headers"); + if (ImGui::TreeNode("Custom headers")) + { + const int COLUMNS_COUNT = 3; + if (ImGui::BeginTable("table_custom_headers", COLUMNS_COUNT, ImGuiTableFlags_Borders | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable)) + { + ImGui::TableSetupColumn("Apricot"); + ImGui::TableSetupColumn("Banana"); + ImGui::TableSetupColumn("Cherry"); + + // Dummy entire-column selection storage + // FIXME: It would be nice to actually demonstrate full-featured selection using those checkbox. + static bool column_selected[3] = {}; + + // Instead of calling TableHeadersRow() we'll submit custom headers ourselves + ImGui::TableNextRow(ImGuiTableRowFlags_Headers); + for (int column = 0; column < COLUMNS_COUNT; column++) + { + ImGui::TableSetColumnIndex(column); + const char* column_name = ImGui::TableGetColumnName(column); // Retrieve name passed to TableSetupColumn() + ImGui::PushID(column); + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(0, 0)); + ImGui::Checkbox("##checkall", &column_selected[column]); + ImGui::PopStyleVar(); + ImGui::SameLine(0.0f, ImGui::GetStyle().ItemInnerSpacing.x); + ImGui::TableHeader(column_name); + ImGui::PopID(); + } + + for (int row = 0; row < 5; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < 3; column++) + { + char buf[32]; + sprintf(buf, "Cell %d,%d", column, row); + ImGui::TableSetColumnIndex(column); + ImGui::Selectable(buf, column_selected[column]); + } + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + // Demonstrate creating custom context menus inside columns, while playing it nice with context menus provided by TableHeadersRow()/TableHeader() + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Context menus"); + if (ImGui::TreeNode("Context menus")) + { + HelpMarker("By default, right-clicking over a TableHeadersRow()/TableHeader() line will open the default context-menu.\nUsing ImGuiTableFlags_ContextMenuInBody we also allow right-clicking over columns body."); + static ImGuiTableFlags flags1 = ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable | ImGuiTableFlags_Borders | ImGuiTableFlags_ContextMenuInBody; + + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_ContextMenuInBody", &flags1, ImGuiTableFlags_ContextMenuInBody); + PopStyleCompact(); + + // Context Menus: first example + // [1.1] Right-click on the TableHeadersRow() line to open the default table context menu. + // [1.2] Right-click in columns also open the default table context menu (if ImGuiTableFlags_ContextMenuInBody is set) + const int COLUMNS_COUNT = 3; + if (ImGui::BeginTable("table_context_menu", COLUMNS_COUNT, flags1)) + { + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + + // [1.1]] Right-click on the TableHeadersRow() line to open the default table context menu. + ImGui::TableHeadersRow(); + + // Submit dummy contents + for (int row = 0; row < 4; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < COLUMNS_COUNT; column++) + { + ImGui::TableSetColumnIndex(column); + ImGui::Text("Cell %d,%d", column, row); + } + } + ImGui::EndTable(); + } + + // Context Menus: second example + // [2.1] Right-click on the TableHeadersRow() line to open the default table context menu. + // [2.2] Right-click on the ".." to open a custom popup + // [2.3] Right-click in columns to open another custom popup + HelpMarker("Demonstrate mixing table context menu (over header), item context button (over button) and custom per-colum context menu (over column body)."); + ImGuiTableFlags flags2 = ImGuiTableFlags_Resizable | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable | ImGuiTableFlags_Borders; + if (ImGui::BeginTable("table_context_menu_2", COLUMNS_COUNT, flags2)) + { + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + + // [2.1] Right-click on the TableHeadersRow() line to open the default table context menu. + ImGui::TableHeadersRow(); + for (int row = 0; row < 4; row++) + { + ImGui::TableNextRow(); + for (int column = 0; column < COLUMNS_COUNT; column++) + { + // Submit dummy contents + ImGui::TableSetColumnIndex(column); + ImGui::Text("Cell %d,%d", column, row); + ImGui::SameLine(); + + // [2.2] Right-click on the ".." to open a custom popup + ImGui::PushID(row * COLUMNS_COUNT + column); + ImGui::SmallButton(".."); + if (ImGui::BeginPopupContextItem()) + { + ImGui::Text("This is the popup for Button(\"..\") in Cell %d,%d", column, row); + if (ImGui::Button("Close")) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + ImGui::PopID(); + } + } + + // [2.3] Right-click anywhere in columns to open another custom popup + // (instead of testing for !IsAnyItemHovered() we could also call OpenPopup() with ImGuiPopupFlags_NoOpenOverExistingPopup + // to manage popup priority as the popups triggers, here "are we hovering a column" are overlapping) + int hovered_column = -1; + for (int column = 0; column < COLUMNS_COUNT + 1; column++) + { + ImGui::PushID(column); + if (ImGui::TableGetColumnFlags(column) & ImGuiTableColumnFlags_IsHovered) + hovered_column = column; + if (hovered_column == column && !ImGui::IsAnyItemHovered() && ImGui::IsMouseReleased(1)) + ImGui::OpenPopup("MyPopup"); + if (ImGui::BeginPopup("MyPopup")) + { + if (column == COLUMNS_COUNT) + ImGui::Text("This is a custom popup for unused space after the last column."); + else + ImGui::Text("This is a custom popup for Column %d", column); + if (ImGui::Button("Close")) + ImGui::CloseCurrentPopup(); + ImGui::EndPopup(); + } + ImGui::PopID(); + } + + ImGui::EndTable(); + ImGui::Text("Hovered column: %d", hovered_column); + } + ImGui::TreePop(); + } + + // Demonstrate creating multiple tables with the same ID + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Synced instances"); + if (ImGui::TreeNode("Synced instances")) + { + HelpMarker("Multiple tables with the same identifier will share their settings, width, visibility, order etc."); + + static ImGuiTableFlags flags = ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable | ImGuiTableFlags_Borders | ImGuiTableFlags_SizingFixedFit | ImGuiTableFlags_NoSavedSettings; + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollY", &flags, ImGuiTableFlags_ScrollY); + ImGui::CheckboxFlags("ImGuiTableFlags_SizingFixedFit", &flags, ImGuiTableFlags_SizingFixedFit); + for (int n = 0; n < 3; n++) + { + char buf[32]; + sprintf(buf, "Synced Table %d", n); + bool open = ImGui::CollapsingHeader(buf, ImGuiTreeNodeFlags_DefaultOpen); + if (open && ImGui::BeginTable("Table", 3, flags, ImVec2(0.0f, ImGui::GetTextLineHeightWithSpacing() * 5))) + { + ImGui::TableSetupColumn("One"); + ImGui::TableSetupColumn("Two"); + ImGui::TableSetupColumn("Three"); + ImGui::TableHeadersRow(); + const int cell_count = (n == 1) ? 27 : 9; // Make second table have a scrollbar to verify that additional decoration is not affecting column positions. + for (int cell = 0; cell < cell_count; cell++) + { + ImGui::TableNextColumn(); + ImGui::Text("this cell %d", cell); + } + ImGui::EndTable(); + } + } + ImGui::TreePop(); + } + + // Demonstrate using Sorting facilities + // This is a simplified version of the "Advanced" example, where we mostly focus on the code necessary to handle sorting. + // Note that the "Advanced" example also showcase manually triggering a sort (e.g. if item quantities have been modified) + static const char* template_items_names[] = + { + "Banana", "Apple", "Cherry", "Watermelon", "Grapefruit", "Strawberry", "Mango", + "Kiwi", "Orange", "Pineapple", "Blueberry", "Plum", "Coconut", "Pear", "Apricot" + }; + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Sorting"); + if (ImGui::TreeNode("Sorting")) + { + // Create item list + static ImVector items; + if (items.Size == 0) + { + items.resize(50, MyItem()); + for (int n = 0; n < items.Size; n++) + { + const int template_n = n % IM_ARRAYSIZE(template_items_names); + MyItem& item = items[n]; + item.ID = n; + item.Name = template_items_names[template_n]; + item.Quantity = (n * n - n) % 20; // Assign default quantities + } + } + + // Options + static ImGuiTableFlags flags = + ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable | ImGuiTableFlags_Sortable | ImGuiTableFlags_SortMulti + | ImGuiTableFlags_RowBg | ImGuiTableFlags_BordersOuter | ImGuiTableFlags_BordersV | ImGuiTableFlags_NoBordersInBody + | ImGuiTableFlags_ScrollY; + PushStyleCompact(); + ImGui::CheckboxFlags("ImGuiTableFlags_SortMulti", &flags, ImGuiTableFlags_SortMulti); + ImGui::SameLine(); HelpMarker("When sorting is enabled: hold shift when clicking headers to sort on multiple column. TableGetSortSpecs() may return specs where (SpecsCount > 1)."); + ImGui::CheckboxFlags("ImGuiTableFlags_SortTristate", &flags, ImGuiTableFlags_SortTristate); + ImGui::SameLine(); HelpMarker("When sorting is enabled: allow no sorting, disable default sorting. TableGetSortSpecs() may return specs where (SpecsCount == 0)."); + PopStyleCompact(); + + if (ImGui::BeginTable("table_sorting", 4, flags, ImVec2(0.0f, TEXT_BASE_HEIGHT * 15), 0.0f)) + { + // Declare columns + // We use the "user_id" parameter of TableSetupColumn() to specify a user id that will be stored in the sort specifications. + // This is so our sort function can identify a column given our own identifier. We could also identify them based on their index! + // Demonstrate using a mixture of flags among available sort-related flags: + // - ImGuiTableColumnFlags_DefaultSort + // - ImGuiTableColumnFlags_NoSort / ImGuiTableColumnFlags_NoSortAscending / ImGuiTableColumnFlags_NoSortDescending + // - ImGuiTableColumnFlags_PreferSortAscending / ImGuiTableColumnFlags_PreferSortDescending + ImGui::TableSetupColumn("ID", ImGuiTableColumnFlags_DefaultSort | ImGuiTableColumnFlags_WidthFixed, 0.0f, MyItemColumnID_ID); + ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_WidthFixed, 0.0f, MyItemColumnID_Name); + ImGui::TableSetupColumn("Action", ImGuiTableColumnFlags_NoSort | ImGuiTableColumnFlags_WidthFixed, 0.0f, MyItemColumnID_Action); + ImGui::TableSetupColumn("Quantity", ImGuiTableColumnFlags_PreferSortDescending | ImGuiTableColumnFlags_WidthStretch, 0.0f, MyItemColumnID_Quantity); + ImGui::TableSetupScrollFreeze(0, 1); // Make row always visible + ImGui::TableHeadersRow(); + + // Sort our data if sort specs have been changed! + if (ImGuiTableSortSpecs* sort_specs = ImGui::TableGetSortSpecs()) + if (sort_specs->SpecsDirty) + { + MyItem::SortWithSortSpecs(sort_specs, items.Data, items.Size); + sort_specs->SpecsDirty = false; + } + + // Demonstrate using clipper for large vertical lists + ImGuiListClipper clipper; + clipper.Begin(items.Size); + while (clipper.Step()) + for (int row_n = clipper.DisplayStart; row_n < clipper.DisplayEnd; row_n++) + { + // Display a data item + MyItem* item = &items[row_n]; + ImGui::PushID(item->ID); + ImGui::TableNextRow(); + ImGui::TableNextColumn(); + ImGui::Text("%04d", item->ID); + ImGui::TableNextColumn(); + ImGui::TextUnformatted(item->Name); + ImGui::TableNextColumn(); + ImGui::SmallButton("None"); + ImGui::TableNextColumn(); + ImGui::Text("%d", item->Quantity); + ImGui::PopID(); + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + + // In this example we'll expose most table flags and settings. + // For specific flags and settings refer to the corresponding section for more detailed explanation. + // This section is mostly useful to experiment with combining certain flags or settings with each others. + //ImGui::SetNextItemOpen(true, ImGuiCond_Once); // [DEBUG] + if (open_action != -1) + ImGui::SetNextItemOpen(open_action != 0); + IMGUI_DEMO_MARKER("Tables/Advanced"); + if (ImGui::TreeNode("Advanced")) + { + static ImGuiTableFlags flags = + ImGuiTableFlags_Resizable | ImGuiTableFlags_Reorderable | ImGuiTableFlags_Hideable + | ImGuiTableFlags_Sortable | ImGuiTableFlags_SortMulti + | ImGuiTableFlags_RowBg | ImGuiTableFlags_Borders | ImGuiTableFlags_NoBordersInBody + | ImGuiTableFlags_ScrollX | ImGuiTableFlags_ScrollY + | ImGuiTableFlags_SizingFixedFit; + + enum ContentsType { CT_Text, CT_Button, CT_SmallButton, CT_FillButton, CT_Selectable, CT_SelectableSpanRow }; + static int contents_type = CT_SelectableSpanRow; + const char* contents_type_names[] = { "Text", "Button", "SmallButton", "FillButton", "Selectable", "Selectable (span row)" }; + static int freeze_cols = 1; + static int freeze_rows = 1; + static int items_count = IM_ARRAYSIZE(template_items_names) * 2; + static ImVec2 outer_size_value = ImVec2(0.0f, TEXT_BASE_HEIGHT * 12); + static float row_min_height = 0.0f; // Auto + static float inner_width_with_scroll = 0.0f; // Auto-extend + static bool outer_size_enabled = true; + static bool show_headers = true; + static bool show_wrapped_text = false; + //static ImGuiTextFilter filter; + //ImGui::SetNextItemOpen(true, ImGuiCond_Once); // FIXME-TABLE: Enabling this results in initial clipped first pass on table which tend to affect column sizing + if (ImGui::TreeNode("Options")) + { + // Make the UI compact because there are so many fields + PushStyleCompact(); + ImGui::PushItemWidth(TEXT_BASE_WIDTH * 28.0f); + + if (ImGui::TreeNodeEx("Features:", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::CheckboxFlags("ImGuiTableFlags_Resizable", &flags, ImGuiTableFlags_Resizable); + ImGui::CheckboxFlags("ImGuiTableFlags_Reorderable", &flags, ImGuiTableFlags_Reorderable); + ImGui::CheckboxFlags("ImGuiTableFlags_Hideable", &flags, ImGuiTableFlags_Hideable); + ImGui::CheckboxFlags("ImGuiTableFlags_Sortable", &flags, ImGuiTableFlags_Sortable); + ImGui::CheckboxFlags("ImGuiTableFlags_NoSavedSettings", &flags, ImGuiTableFlags_NoSavedSettings); + ImGui::CheckboxFlags("ImGuiTableFlags_ContextMenuInBody", &flags, ImGuiTableFlags_ContextMenuInBody); + ImGui::TreePop(); + } + + if (ImGui::TreeNodeEx("Decorations:", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::CheckboxFlags("ImGuiTableFlags_RowBg", &flags, ImGuiTableFlags_RowBg); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersV", &flags, ImGuiTableFlags_BordersV); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuterV", &flags, ImGuiTableFlags_BordersOuterV); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInnerV", &flags, ImGuiTableFlags_BordersInnerV); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersH", &flags, ImGuiTableFlags_BordersH); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersOuterH", &flags, ImGuiTableFlags_BordersOuterH); + ImGui::CheckboxFlags("ImGuiTableFlags_BordersInnerH", &flags, ImGuiTableFlags_BordersInnerH); + ImGui::CheckboxFlags("ImGuiTableFlags_NoBordersInBody", &flags, ImGuiTableFlags_NoBordersInBody); ImGui::SameLine(); HelpMarker("Disable vertical borders in columns Body (borders will always appear in Headers"); + ImGui::CheckboxFlags("ImGuiTableFlags_NoBordersInBodyUntilResize", &flags, ImGuiTableFlags_NoBordersInBodyUntilResize); ImGui::SameLine(); HelpMarker("Disable vertical borders in columns Body until hovered for resize (borders will always appear in Headers)"); + ImGui::TreePop(); + } + + if (ImGui::TreeNodeEx("Sizing:", ImGuiTreeNodeFlags_DefaultOpen)) + { + EditTableSizingFlags(&flags); + ImGui::SameLine(); HelpMarker("In the Advanced demo we override the policy of each column so those table-wide settings have less effect that typical."); + ImGui::CheckboxFlags("ImGuiTableFlags_NoHostExtendX", &flags, ImGuiTableFlags_NoHostExtendX); + ImGui::SameLine(); HelpMarker("Make outer width auto-fit to columns, overriding outer_size.x value.\n\nOnly available when ScrollX/ScrollY are disabled and Stretch columns are not used."); + ImGui::CheckboxFlags("ImGuiTableFlags_NoHostExtendY", &flags, ImGuiTableFlags_NoHostExtendY); + ImGui::SameLine(); HelpMarker("Make outer height stop exactly at outer_size.y (prevent auto-extending table past the limit).\n\nOnly available when ScrollX/ScrollY are disabled. Data below the limit will be clipped and not visible."); + ImGui::CheckboxFlags("ImGuiTableFlags_NoKeepColumnsVisible", &flags, ImGuiTableFlags_NoKeepColumnsVisible); + ImGui::SameLine(); HelpMarker("Only available if ScrollX is disabled."); + ImGui::CheckboxFlags("ImGuiTableFlags_PreciseWidths", &flags, ImGuiTableFlags_PreciseWidths); + ImGui::SameLine(); HelpMarker("Disable distributing remainder width to stretched columns (width allocation on a 100-wide table with 3 columns: Without this flag: 33,33,34. With this flag: 33,33,33). With larger number of columns, resizing will appear to be less smooth."); + ImGui::CheckboxFlags("ImGuiTableFlags_NoClip", &flags, ImGuiTableFlags_NoClip); + ImGui::SameLine(); HelpMarker("Disable clipping rectangle for every individual columns (reduce draw command count, items will be able to overflow into other columns). Generally incompatible with ScrollFreeze options."); + ImGui::TreePop(); + } + + if (ImGui::TreeNodeEx("Padding:", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::CheckboxFlags("ImGuiTableFlags_PadOuterX", &flags, ImGuiTableFlags_PadOuterX); + ImGui::CheckboxFlags("ImGuiTableFlags_NoPadOuterX", &flags, ImGuiTableFlags_NoPadOuterX); + ImGui::CheckboxFlags("ImGuiTableFlags_NoPadInnerX", &flags, ImGuiTableFlags_NoPadInnerX); + ImGui::TreePop(); + } + + if (ImGui::TreeNodeEx("Scrolling:", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollX", &flags, ImGuiTableFlags_ScrollX); + ImGui::SameLine(); + ImGui::SetNextItemWidth(ImGui::GetFrameHeight()); + ImGui::DragInt("freeze_cols", &freeze_cols, 0.2f, 0, 9, NULL, ImGuiSliderFlags_NoInput); + ImGui::CheckboxFlags("ImGuiTableFlags_ScrollY", &flags, ImGuiTableFlags_ScrollY); + ImGui::SameLine(); + ImGui::SetNextItemWidth(ImGui::GetFrameHeight()); + ImGui::DragInt("freeze_rows", &freeze_rows, 0.2f, 0, 9, NULL, ImGuiSliderFlags_NoInput); + ImGui::TreePop(); + } + + if (ImGui::TreeNodeEx("Sorting:", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::CheckboxFlags("ImGuiTableFlags_SortMulti", &flags, ImGuiTableFlags_SortMulti); + ImGui::SameLine(); HelpMarker("When sorting is enabled: hold shift when clicking headers to sort on multiple column. TableGetSortSpecs() may return specs where (SpecsCount > 1)."); + ImGui::CheckboxFlags("ImGuiTableFlags_SortTristate", &flags, ImGuiTableFlags_SortTristate); + ImGui::SameLine(); HelpMarker("When sorting is enabled: allow no sorting, disable default sorting. TableGetSortSpecs() may return specs where (SpecsCount == 0)."); + ImGui::TreePop(); + } + + if (ImGui::TreeNodeEx("Other:", ImGuiTreeNodeFlags_DefaultOpen)) + { + ImGui::Checkbox("show_headers", &show_headers); + ImGui::Checkbox("show_wrapped_text", &show_wrapped_text); + + ImGui::DragFloat2("##OuterSize", &outer_size_value.x); + ImGui::SameLine(0.0f, ImGui::GetStyle().ItemInnerSpacing.x); + ImGui::Checkbox("outer_size", &outer_size_enabled); + ImGui::SameLine(); + HelpMarker("If scrolling is disabled (ScrollX and ScrollY not set):\n" + "- The table is output directly in the parent window.\n" + "- OuterSize.x < 0.0f will right-align the table.\n" + "- OuterSize.x = 0.0f will narrow fit the table unless there are any Stretch columns.\n" + "- OuterSize.y then becomes the minimum size for the table, which will extend vertically if there are more rows (unless NoHostExtendY is set)."); + + // From a user point of view we will tend to use 'inner_width' differently depending on whether our table is embedding scrolling. + // To facilitate toying with this demo we will actually pass 0.0f to the BeginTable() when ScrollX is disabled. + ImGui::DragFloat("inner_width (when ScrollX active)", &inner_width_with_scroll, 1.0f, 0.0f, FLT_MAX); + + ImGui::DragFloat("row_min_height", &row_min_height, 1.0f, 0.0f, FLT_MAX); + ImGui::SameLine(); HelpMarker("Specify height of the Selectable item."); + + ImGui::DragInt("items_count", &items_count, 0.1f, 0, 9999); + ImGui::Combo("items_type (first column)", &contents_type, contents_type_names, IM_ARRAYSIZE(contents_type_names)); + //filter.Draw("filter"); + ImGui::TreePop(); + } + + ImGui::PopItemWidth(); + PopStyleCompact(); + ImGui::Spacing(); + ImGui::TreePop(); + } + + // Update item list if we changed the number of items + static ImVector items; + static ImVector selection; + static bool items_need_sort = false; + if (items.Size != items_count) + { + items.resize(items_count, MyItem()); + for (int n = 0; n < items_count; n++) + { + const int template_n = n % IM_ARRAYSIZE(template_items_names); + MyItem& item = items[n]; + item.ID = n; + item.Name = template_items_names[template_n]; + item.Quantity = (template_n == 3) ? 10 : (template_n == 4) ? 20 : 0; // Assign default quantities + } + } + + const ImDrawList* parent_draw_list = ImGui::GetWindowDrawList(); + const int parent_draw_list_draw_cmd_count = parent_draw_list->CmdBuffer.Size; + ImVec2 table_scroll_cur, table_scroll_max; // For debug display + const ImDrawList* table_draw_list = NULL; // " + + // Submit table + const float inner_width_to_use = (flags & ImGuiTableFlags_ScrollX) ? inner_width_with_scroll : 0.0f; + if (ImGui::BeginTable("table_advanced", 6, flags, outer_size_enabled ? outer_size_value : ImVec2(0, 0), inner_width_to_use)) + { + // Declare columns + // We use the "user_id" parameter of TableSetupColumn() to specify a user id that will be stored in the sort specifications. + // This is so our sort function can identify a column given our own identifier. We could also identify them based on their index! + ImGui::TableSetupColumn("ID", ImGuiTableColumnFlags_DefaultSort | ImGuiTableColumnFlags_WidthFixed | ImGuiTableColumnFlags_NoHide, 0.0f, MyItemColumnID_ID); + ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_WidthFixed, 0.0f, MyItemColumnID_Name); + ImGui::TableSetupColumn("Action", ImGuiTableColumnFlags_NoSort | ImGuiTableColumnFlags_WidthFixed, 0.0f, MyItemColumnID_Action); + ImGui::TableSetupColumn("Quantity", ImGuiTableColumnFlags_PreferSortDescending, 0.0f, MyItemColumnID_Quantity); + ImGui::TableSetupColumn("Description", (flags & ImGuiTableFlags_NoHostExtendX) ? 0 : ImGuiTableColumnFlags_WidthStretch, 0.0f, MyItemColumnID_Description); + ImGui::TableSetupColumn("Hidden", ImGuiTableColumnFlags_DefaultHide | ImGuiTableColumnFlags_NoSort); + ImGui::TableSetupScrollFreeze(freeze_cols, freeze_rows); + + // Sort our data if sort specs have been changed! + ImGuiTableSortSpecs* sort_specs = ImGui::TableGetSortSpecs(); + if (sort_specs && sort_specs->SpecsDirty) + items_need_sort = true; + if (sort_specs && items_need_sort && items.Size > 1) + { + MyItem::SortWithSortSpecs(sort_specs, items.Data, items.Size); + sort_specs->SpecsDirty = false; + } + items_need_sort = false; + + // Take note of whether we are currently sorting based on the Quantity field, + // we will use this to trigger sorting when we know the data of this column has been modified. + const bool sorts_specs_using_quantity = (ImGui::TableGetColumnFlags(3) & ImGuiTableColumnFlags_IsSorted) != 0; + + // Show headers + if (show_headers) + ImGui::TableHeadersRow(); + + // Show data + // FIXME-TABLE FIXME-NAV: How we can get decent up/down even though we have the buttons here? + ImGui::PushButtonRepeat(true); +#if 1 + // Demonstrate using clipper for large vertical lists + ImGuiListClipper clipper; + clipper.Begin(items.Size); + while (clipper.Step()) + { + for (int row_n = clipper.DisplayStart; row_n < clipper.DisplayEnd; row_n++) +#else + // Without clipper + { + for (int row_n = 0; row_n < items.Size; row_n++) +#endif + { + MyItem* item = &items[row_n]; + //if (!filter.PassFilter(item->Name)) + // continue; + + const bool item_is_selected = selection.contains(item->ID); + ImGui::PushID(item->ID); + ImGui::TableNextRow(ImGuiTableRowFlags_None, row_min_height); + + // For the demo purpose we can select among different type of items submitted in the first column + ImGui::TableSetColumnIndex(0); + char label[32]; + sprintf(label, "%04d", item->ID); + if (contents_type == CT_Text) + ImGui::TextUnformatted(label); + else if (contents_type == CT_Button) + ImGui::Button(label); + else if (contents_type == CT_SmallButton) + ImGui::SmallButton(label); + else if (contents_type == CT_FillButton) + ImGui::Button(label, ImVec2(-FLT_MIN, 0.0f)); + else if (contents_type == CT_Selectable || contents_type == CT_SelectableSpanRow) + { + ImGuiSelectableFlags selectable_flags = (contents_type == CT_SelectableSpanRow) ? ImGuiSelectableFlags_SpanAllColumns | ImGuiSelectableFlags_AllowOverlap : ImGuiSelectableFlags_None; + if (ImGui::Selectable(label, item_is_selected, selectable_flags, ImVec2(0, row_min_height))) + { + if (ImGui::GetIO().KeyCtrl) + { + if (item_is_selected) + selection.find_erase_unsorted(item->ID); + else + selection.push_back(item->ID); + } + else + { + selection.clear(); + selection.push_back(item->ID); + } + } + } + + if (ImGui::TableSetColumnIndex(1)) + ImGui::TextUnformatted(item->Name); + + // Here we demonstrate marking our data set as needing to be sorted again if we modified a quantity, + // and we are currently sorting on the column showing the Quantity. + // To avoid triggering a sort while holding the button, we only trigger it when the button has been released. + // You will probably need a more advanced system in your code if you want to automatically sort when a specific entry changes. + if (ImGui::TableSetColumnIndex(2)) + { + if (ImGui::SmallButton("Chop")) { item->Quantity += 1; } + if (sorts_specs_using_quantity && ImGui::IsItemDeactivated()) { items_need_sort = true; } + ImGui::SameLine(); + if (ImGui::SmallButton("Eat")) { item->Quantity -= 1; } + if (sorts_specs_using_quantity && ImGui::IsItemDeactivated()) { items_need_sort = true; } + } + + if (ImGui::TableSetColumnIndex(3)) + ImGui::Text("%d", item->Quantity); + + ImGui::TableSetColumnIndex(4); + if (show_wrapped_text) + ImGui::TextWrapped("Lorem ipsum dolor sit amet"); + else + ImGui::Text("Lorem ipsum dolor sit amet"); + + if (ImGui::TableSetColumnIndex(5)) + ImGui::Text("1234"); + + ImGui::PopID(); + } + } + ImGui::PopButtonRepeat(); + + // Store some info to display debug details below + table_scroll_cur = ImVec2(ImGui::GetScrollX(), ImGui::GetScrollY()); + table_scroll_max = ImVec2(ImGui::GetScrollMaxX(), ImGui::GetScrollMaxY()); + table_draw_list = ImGui::GetWindowDrawList(); + ImGui::EndTable(); + } + static bool show_debug_details = false; + ImGui::Checkbox("Debug details", &show_debug_details); + if (show_debug_details && table_draw_list) + { + ImGui::SameLine(0.0f, 0.0f); + const int table_draw_list_draw_cmd_count = table_draw_list->CmdBuffer.Size; + if (table_draw_list == parent_draw_list) + ImGui::Text(": DrawCmd: +%d (in same window)", + table_draw_list_draw_cmd_count - parent_draw_list_draw_cmd_count); + else + ImGui::Text(": DrawCmd: +%d (in child window), Scroll: (%.f/%.f) (%.f/%.f)", + table_draw_list_draw_cmd_count - 1, table_scroll_cur.x, table_scroll_max.x, table_scroll_cur.y, table_scroll_max.y); + } + ImGui::TreePop(); + } + + ImGui::PopID(); + + ShowDemoWindowColumns(); + + if (disable_indent) + ImGui::PopStyleVar(); +} + +// Demonstrate old/legacy Columns API! +// [2020: Columns are under-featured and not maintained. Prefer using the more flexible and powerful BeginTable() API!] +static void ShowDemoWindowColumns() +{ + IMGUI_DEMO_MARKER("Columns (legacy API)"); + bool open = ImGui::TreeNode("Legacy Columns API"); + ImGui::SameLine(); + HelpMarker("Columns() is an old API! Prefer using the more flexible and powerful BeginTable() API!"); + if (!open) + return; + + // Basic columns + IMGUI_DEMO_MARKER("Columns (legacy API)/Basic"); + if (ImGui::TreeNode("Basic")) + { + ImGui::Text("Without border:"); + ImGui::Columns(3, "mycolumns3", false); // 3-ways, no border + ImGui::Separator(); + for (int n = 0; n < 14; n++) + { + char label[32]; + sprintf(label, "Item %d", n); + if (ImGui::Selectable(label)) {} + //if (ImGui::Button(label, ImVec2(-FLT_MIN,0.0f))) {} + ImGui::NextColumn(); + } + ImGui::Columns(1); + ImGui::Separator(); + + ImGui::Text("With border:"); + ImGui::Columns(4, "mycolumns"); // 4-ways, with border + ImGui::Separator(); + ImGui::Text("ID"); ImGui::NextColumn(); + ImGui::Text("Name"); ImGui::NextColumn(); + ImGui::Text("Path"); ImGui::NextColumn(); + ImGui::Text("Hovered"); ImGui::NextColumn(); + ImGui::Separator(); + const char* names[3] = { "One", "Two", "Three" }; + const char* paths[3] = { "/path/one", "/path/two", "/path/three" }; + static int selected = -1; + for (int i = 0; i < 3; i++) + { + char label[32]; + sprintf(label, "%04d", i); + if (ImGui::Selectable(label, selected == i, ImGuiSelectableFlags_SpanAllColumns)) + selected = i; + bool hovered = ImGui::IsItemHovered(); + ImGui::NextColumn(); + ImGui::Text(names[i]); ImGui::NextColumn(); + ImGui::Text(paths[i]); ImGui::NextColumn(); + ImGui::Text("%d", hovered); ImGui::NextColumn(); + } + ImGui::Columns(1); + ImGui::Separator(); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Columns (legacy API)/Borders"); + if (ImGui::TreeNode("Borders")) + { + // NB: Future columns API should allow automatic horizontal borders. + static bool h_borders = true; + static bool v_borders = true; + static int columns_count = 4; + const int lines_count = 3; + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8); + ImGui::DragInt("##columns_count", &columns_count, 0.1f, 2, 10, "%d columns"); + if (columns_count < 2) + columns_count = 2; + ImGui::SameLine(); + ImGui::Checkbox("horizontal", &h_borders); + ImGui::SameLine(); + ImGui::Checkbox("vertical", &v_borders); + ImGui::Columns(columns_count, NULL, v_borders); + for (int i = 0; i < columns_count * lines_count; i++) + { + if (h_borders && ImGui::GetColumnIndex() == 0) + ImGui::Separator(); + ImGui::Text("%c%c%c", 'a' + i, 'a' + i, 'a' + i); + ImGui::Text("Width %.2f", ImGui::GetColumnWidth()); + ImGui::Text("Avail %.2f", ImGui::GetContentRegionAvail().x); + ImGui::Text("Offset %.2f", ImGui::GetColumnOffset()); + ImGui::Text("Long text that is likely to clip"); + ImGui::Button("Button", ImVec2(-FLT_MIN, 0.0f)); + ImGui::NextColumn(); + } + ImGui::Columns(1); + if (h_borders) + ImGui::Separator(); + ImGui::TreePop(); + } + + // Create multiple items in a same cell before switching to next column + IMGUI_DEMO_MARKER("Columns (legacy API)/Mixed items"); + if (ImGui::TreeNode("Mixed items")) + { + ImGui::Columns(3, "mixed"); + ImGui::Separator(); + + ImGui::Text("Hello"); + ImGui::Button("Banana"); + ImGui::NextColumn(); + + ImGui::Text("ImGui"); + ImGui::Button("Apple"); + static float foo = 1.0f; + ImGui::InputFloat("red", &foo, 0.05f, 0, "%.3f"); + ImGui::Text("An extra line here."); + ImGui::NextColumn(); + + ImGui::Text("Sailor"); + ImGui::Button("Corniflower"); + static float bar = 1.0f; + ImGui::InputFloat("blue", &bar, 0.05f, 0, "%.3f"); + ImGui::NextColumn(); + + if (ImGui::CollapsingHeader("Category A")) { ImGui::Text("Blah blah blah"); } ImGui::NextColumn(); + if (ImGui::CollapsingHeader("Category B")) { ImGui::Text("Blah blah blah"); } ImGui::NextColumn(); + if (ImGui::CollapsingHeader("Category C")) { ImGui::Text("Blah blah blah"); } ImGui::NextColumn(); + ImGui::Columns(1); + ImGui::Separator(); + ImGui::TreePop(); + } + + // Word wrapping + IMGUI_DEMO_MARKER("Columns (legacy API)/Word-wrapping"); + if (ImGui::TreeNode("Word-wrapping")) + { + ImGui::Columns(2, "word-wrapping"); + ImGui::Separator(); + ImGui::TextWrapped("The quick brown fox jumps over the lazy dog."); + ImGui::TextWrapped("Hello Left"); + ImGui::NextColumn(); + ImGui::TextWrapped("The quick brown fox jumps over the lazy dog."); + ImGui::TextWrapped("Hello Right"); + ImGui::Columns(1); + ImGui::Separator(); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Columns (legacy API)/Horizontal Scrolling"); + if (ImGui::TreeNode("Horizontal Scrolling")) + { + ImGui::SetNextWindowContentSize(ImVec2(1500.0f, 0.0f)); + ImVec2 child_size = ImVec2(0, ImGui::GetFontSize() * 20.0f); + ImGui::BeginChild("##ScrollingRegion", child_size, false, ImGuiWindowFlags_HorizontalScrollbar); + ImGui::Columns(10); + + // Also demonstrate using clipper for large vertical lists + int ITEMS_COUNT = 2000; + ImGuiListClipper clipper; + clipper.Begin(ITEMS_COUNT); + while (clipper.Step()) + { + for (int i = clipper.DisplayStart; i < clipper.DisplayEnd; i++) + for (int j = 0; j < 10; j++) + { + ImGui::Text("Line %d Column %d...", i, j); + ImGui::NextColumn(); + } + } + ImGui::Columns(1); + ImGui::EndChild(); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Columns (legacy API)/Tree"); + if (ImGui::TreeNode("Tree")) + { + ImGui::Columns(2, "tree", true); + for (int x = 0; x < 3; x++) + { + bool open1 = ImGui::TreeNode((void*)(intptr_t)x, "Node%d", x); + ImGui::NextColumn(); + ImGui::Text("Node contents"); + ImGui::NextColumn(); + if (open1) + { + for (int y = 0; y < 3; y++) + { + bool open2 = ImGui::TreeNode((void*)(intptr_t)y, "Node%d.%d", x, y); + ImGui::NextColumn(); + ImGui::Text("Node contents"); + if (open2) + { + ImGui::Text("Even more contents"); + if (ImGui::TreeNode("Tree in column")) + { + ImGui::Text("The quick brown fox jumps over the lazy dog"); + ImGui::TreePop(); + } + } + ImGui::NextColumn(); + if (open2) + ImGui::TreePop(); + } + ImGui::TreePop(); + } + } + ImGui::Columns(1); + ImGui::TreePop(); + } + + ImGui::TreePop(); +} + +static void ShowDemoWindowInputs() +{ + IMGUI_DEMO_MARKER("Inputs & Focus"); + if (ImGui::CollapsingHeader("Inputs & Focus")) + { + ImGuiIO& io = ImGui::GetIO(); + + // Display inputs submitted to ImGuiIO + IMGUI_DEMO_MARKER("Inputs & Focus/Inputs"); + ImGui::SetNextItemOpen(true, ImGuiCond_Once); + if (ImGui::TreeNode("Inputs")) + { + HelpMarker( + "This is a simplified view. See more detailed input state:\n" + "- in 'Tools->Metrics/Debugger->Inputs'.\n" + "- in 'Tools->Debug Log->IO'."); + if (ImGui::IsMousePosValid()) + ImGui::Text("Mouse pos: (%g, %g)", io.MousePos.x, io.MousePos.y); + else + ImGui::Text("Mouse pos: "); + ImGui::Text("Mouse delta: (%g, %g)", io.MouseDelta.x, io.MouseDelta.y); + ImGui::Text("Mouse down:"); + for (int i = 0; i < IM_ARRAYSIZE(io.MouseDown); i++) if (ImGui::IsMouseDown(i)) { ImGui::SameLine(); ImGui::Text("b%d (%.02f secs)", i, io.MouseDownDuration[i]); } + ImGui::Text("Mouse wheel: %.1f", io.MouseWheel); + + // We iterate both legacy native range and named ImGuiKey ranges, which is a little odd but this allows displaying the data for old/new backends. + // User code should never have to go through such hoops! You can generally iterate between ImGuiKey_NamedKey_BEGIN and ImGuiKey_NamedKey_END. +#ifdef IMGUI_DISABLE_OBSOLETE_KEYIO + struct funcs { static bool IsLegacyNativeDupe(ImGuiKey) { return false; } }; + ImGuiKey start_key = ImGuiKey_NamedKey_BEGIN; +#else + struct funcs { static bool IsLegacyNativeDupe(ImGuiKey key) { return key < 512 && ImGui::GetIO().KeyMap[key] != -1; } }; // Hide Native<>ImGuiKey duplicates when both exists in the array + ImGuiKey start_key = (ImGuiKey)0; +#endif + ImGui::Text("Keys down:"); for (ImGuiKey key = start_key; key < ImGuiKey_NamedKey_END; key = (ImGuiKey)(key + 1)) { if (funcs::IsLegacyNativeDupe(key) || !ImGui::IsKeyDown(key)) continue; ImGui::SameLine(); ImGui::Text((key < ImGuiKey_NamedKey_BEGIN) ? "\"%s\"" : "\"%s\" %d", ImGui::GetKeyName(key), key); } + ImGui::Text("Keys mods: %s%s%s%s", io.KeyCtrl ? "CTRL " : "", io.KeyShift ? "SHIFT " : "", io.KeyAlt ? "ALT " : "", io.KeySuper ? "SUPER " : ""); + ImGui::Text("Chars queue:"); for (int i = 0; i < io.InputQueueCharacters.Size; i++) { ImWchar c = io.InputQueueCharacters[i]; ImGui::SameLine(); ImGui::Text("\'%c\' (0x%04X)", (c > ' ' && c <= 255) ? (char)c : '?', c); } // FIXME: We should convert 'c' to UTF-8 here but the functions are not public. + + ImGui::TreePop(); + } + + // Display ImGuiIO output flags + IMGUI_DEMO_MARKER("Inputs & Focus/Outputs"); + ImGui::SetNextItemOpen(true, ImGuiCond_Once); + if (ImGui::TreeNode("Outputs")) + { + HelpMarker( + "The value of io.WantCaptureMouse and io.WantCaptureKeyboard are normally set by Dear ImGui " + "to instruct your application of how to route inputs. Typically, when a value is true, it means " + "Dear ImGui wants the corresponding inputs and we expect the underlying application to ignore them.\n\n" + "The most typical case is: when hovering a window, Dear ImGui set io.WantCaptureMouse to true, " + "and underlying application should ignore mouse inputs (in practice there are many and more subtle " + "rules leading to how those flags are set)."); + ImGui::Text("io.WantCaptureMouse: %d", io.WantCaptureMouse); + ImGui::Text("io.WantCaptureMouseUnlessPopupClose: %d", io.WantCaptureMouseUnlessPopupClose); + ImGui::Text("io.WantCaptureKeyboard: %d", io.WantCaptureKeyboard); + ImGui::Text("io.WantTextInput: %d", io.WantTextInput); + ImGui::Text("io.WantSetMousePos: %d", io.WantSetMousePos); + ImGui::Text("io.NavActive: %d, io.NavVisible: %d", io.NavActive, io.NavVisible); + + IMGUI_DEMO_MARKER("Inputs & Focus/Outputs/WantCapture override"); + if (ImGui::TreeNode("WantCapture override")) + { + HelpMarker( + "Hovering the colored canvas will override io.WantCaptureXXX fields.\n" + "Notice how normally (when set to none), the value of io.WantCaptureKeyboard would be false when hovering and true when clicking."); + static int capture_override_mouse = -1; + static int capture_override_keyboard = -1; + const char* capture_override_desc[] = { "None", "Set to false", "Set to true" }; + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 15); + ImGui::SliderInt("SetNextFrameWantCaptureMouse() on hover", &capture_override_mouse, -1, +1, capture_override_desc[capture_override_mouse + 1], ImGuiSliderFlags_AlwaysClamp); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 15); + ImGui::SliderInt("SetNextFrameWantCaptureKeyboard() on hover", &capture_override_keyboard, -1, +1, capture_override_desc[capture_override_keyboard + 1], ImGuiSliderFlags_AlwaysClamp); + + ImGui::ColorButton("##panel", ImVec4(0.7f, 0.1f, 0.7f, 1.0f), ImGuiColorEditFlags_NoTooltip | ImGuiColorEditFlags_NoDragDrop, ImVec2(128.0f, 96.0f)); // Dummy item + if (ImGui::IsItemHovered() && capture_override_mouse != -1) + ImGui::SetNextFrameWantCaptureMouse(capture_override_mouse == 1); + if (ImGui::IsItemHovered() && capture_override_keyboard != -1) + ImGui::SetNextFrameWantCaptureKeyboard(capture_override_keyboard == 1); + + ImGui::TreePop(); + } + ImGui::TreePop(); + } + + // Display mouse cursors + IMGUI_DEMO_MARKER("Inputs & Focus/Mouse Cursors"); + if (ImGui::TreeNode("Mouse Cursors")) + { + const char* mouse_cursors_names[] = { "Arrow", "TextInput", "ResizeAll", "ResizeNS", "ResizeEW", "ResizeNESW", "ResizeNWSE", "Hand", "NotAllowed" }; + IM_ASSERT(IM_ARRAYSIZE(mouse_cursors_names) == ImGuiMouseCursor_COUNT); + + ImGuiMouseCursor current = ImGui::GetMouseCursor(); + ImGui::Text("Current mouse cursor = %d: %s", current, mouse_cursors_names[current]); + ImGui::BeginDisabled(true); + ImGui::CheckboxFlags("io.BackendFlags: HasMouseCursors", &io.BackendFlags, ImGuiBackendFlags_HasMouseCursors); + ImGui::EndDisabled(); + + ImGui::Text("Hover to see mouse cursors:"); + ImGui::SameLine(); HelpMarker( + "Your application can render a different mouse cursor based on what ImGui::GetMouseCursor() returns. " + "If software cursor rendering (io.MouseDrawCursor) is set ImGui will draw the right cursor for you, " + "otherwise your backend needs to handle it."); + for (int i = 0; i < ImGuiMouseCursor_COUNT; i++) + { + char label[32]; + sprintf(label, "Mouse cursor %d: %s", i, mouse_cursors_names[i]); + ImGui::Bullet(); ImGui::Selectable(label, false); + if (ImGui::IsItemHovered()) + ImGui::SetMouseCursor(i); + } + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Inputs & Focus/Tabbing"); + if (ImGui::TreeNode("Tabbing")) + { + ImGui::Text("Use TAB/SHIFT+TAB to cycle through keyboard editable fields."); + static char buf[32] = "hello"; + ImGui::InputText("1", buf, IM_ARRAYSIZE(buf)); + ImGui::InputText("2", buf, IM_ARRAYSIZE(buf)); + ImGui::InputText("3", buf, IM_ARRAYSIZE(buf)); + ImGui::PushTabStop(false); + ImGui::InputText("4 (tab skip)", buf, IM_ARRAYSIZE(buf)); + ImGui::SameLine(); HelpMarker("Item won't be cycled through when using TAB or Shift+Tab."); + ImGui::PopTabStop(); + ImGui::InputText("5", buf, IM_ARRAYSIZE(buf)); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Inputs & Focus/Focus from code"); + if (ImGui::TreeNode("Focus from code")) + { + bool focus_1 = ImGui::Button("Focus on 1"); ImGui::SameLine(); + bool focus_2 = ImGui::Button("Focus on 2"); ImGui::SameLine(); + bool focus_3 = ImGui::Button("Focus on 3"); + int has_focus = 0; + static char buf[128] = "click on a button to set focus"; + + if (focus_1) ImGui::SetKeyboardFocusHere(); + ImGui::InputText("1", buf, IM_ARRAYSIZE(buf)); + if (ImGui::IsItemActive()) has_focus = 1; + + if (focus_2) ImGui::SetKeyboardFocusHere(); + ImGui::InputText("2", buf, IM_ARRAYSIZE(buf)); + if (ImGui::IsItemActive()) has_focus = 2; + + ImGui::PushTabStop(false); + if (focus_3) ImGui::SetKeyboardFocusHere(); + ImGui::InputText("3 (tab skip)", buf, IM_ARRAYSIZE(buf)); + if (ImGui::IsItemActive()) has_focus = 3; + ImGui::SameLine(); HelpMarker("Item won't be cycled through when using TAB or Shift+Tab."); + ImGui::PopTabStop(); + + if (has_focus) + ImGui::Text("Item with focus: %d", has_focus); + else + ImGui::Text("Item with focus: "); + + // Use >= 0 parameter to SetKeyboardFocusHere() to focus an upcoming item + static float f3[3] = { 0.0f, 0.0f, 0.0f }; + int focus_ahead = -1; + if (ImGui::Button("Focus on X")) { focus_ahead = 0; } ImGui::SameLine(); + if (ImGui::Button("Focus on Y")) { focus_ahead = 1; } ImGui::SameLine(); + if (ImGui::Button("Focus on Z")) { focus_ahead = 2; } + if (focus_ahead != -1) ImGui::SetKeyboardFocusHere(focus_ahead); + ImGui::SliderFloat3("Float3", &f3[0], 0.0f, 1.0f); + + ImGui::TextWrapped("NB: Cursor & selection are preserved when refocusing last used item in code."); + ImGui::TreePop(); + } + + IMGUI_DEMO_MARKER("Inputs & Focus/Dragging"); + if (ImGui::TreeNode("Dragging")) + { + ImGui::TextWrapped("You can use ImGui::GetMouseDragDelta(0) to query for the dragged amount on any widget."); + for (int button = 0; button < 3; button++) + { + ImGui::Text("IsMouseDragging(%d):", button); + ImGui::Text(" w/ default threshold: %d,", ImGui::IsMouseDragging(button)); + ImGui::Text(" w/ zero threshold: %d,", ImGui::IsMouseDragging(button, 0.0f)); + ImGui::Text(" w/ large threshold: %d,", ImGui::IsMouseDragging(button, 20.0f)); + } + + ImGui::Button("Drag Me"); + if (ImGui::IsItemActive()) + ImGui::GetForegroundDrawList()->AddLine(io.MouseClickedPos[0], io.MousePos, ImGui::GetColorU32(ImGuiCol_Button), 4.0f); // Draw a line between the button and the mouse cursor + + // Drag operations gets "unlocked" when the mouse has moved past a certain threshold + // (the default threshold is stored in io.MouseDragThreshold). You can request a lower or higher + // threshold using the second parameter of IsMouseDragging() and GetMouseDragDelta(). + ImVec2 value_raw = ImGui::GetMouseDragDelta(0, 0.0f); + ImVec2 value_with_lock_threshold = ImGui::GetMouseDragDelta(0); + ImVec2 mouse_delta = io.MouseDelta; + ImGui::Text("GetMouseDragDelta(0):"); + ImGui::Text(" w/ default threshold: (%.1f, %.1f)", value_with_lock_threshold.x, value_with_lock_threshold.y); + ImGui::Text(" w/ zero threshold: (%.1f, %.1f)", value_raw.x, value_raw.y); + ImGui::Text("io.MouseDelta: (%.1f, %.1f)", mouse_delta.x, mouse_delta.y); + ImGui::TreePop(); + } + } +} + +//----------------------------------------------------------------------------- +// [SECTION] About Window / ShowAboutWindow() +// Access from Dear ImGui Demo -> Tools -> About +//----------------------------------------------------------------------------- + +void ImGui::ShowAboutWindow(bool* p_open) +{ + if (!ImGui::Begin("About Dear ImGui", p_open, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::End(); + return; + } + IMGUI_DEMO_MARKER("Tools/About Dear ImGui"); + ImGui::Text("Dear ImGui %s (%d)", IMGUI_VERSION, IMGUI_VERSION_NUM); + ImGui::Separator(); + ImGui::Text("By Omar Cornut and all Dear ImGui contributors."); + ImGui::Text("Dear ImGui is licensed under the MIT License, see LICENSE for more information."); + ImGui::Text("If your company uses this, please consider sponsoring the project!"); + + static bool show_config_info = false; + ImGui::Checkbox("Config/Build Information", &show_config_info); + if (show_config_info) + { + ImGuiIO& io = ImGui::GetIO(); + ImGuiStyle& style = ImGui::GetStyle(); + + bool copy_to_clipboard = ImGui::Button("Copy to clipboard"); + ImVec2 child_size = ImVec2(0, ImGui::GetTextLineHeightWithSpacing() * 18); + ImGui::BeginChildFrame(ImGui::GetID("cfg_infos"), child_size, ImGuiWindowFlags_NoMove); + if (copy_to_clipboard) + { + ImGui::LogToClipboard(); + ImGui::LogText("```\n"); // Back quotes will make text appears without formatting when pasting on GitHub + } + + ImGui::Text("Dear ImGui %s (%d)", IMGUI_VERSION, IMGUI_VERSION_NUM); + ImGui::Separator(); + ImGui::Text("sizeof(size_t): %d, sizeof(ImDrawIdx): %d, sizeof(ImDrawVert): %d", (int)sizeof(size_t), (int)sizeof(ImDrawIdx), (int)sizeof(ImDrawVert)); + ImGui::Text("define: __cplusplus=%d", (int)__cplusplus); +#ifdef IMGUI_DISABLE_OBSOLETE_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_OBSOLETE_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_OBSOLETE_KEYIO + ImGui::Text("define: IMGUI_DISABLE_OBSOLETE_KEYIO"); +#endif +#ifdef IMGUI_DISABLE_WIN32_DEFAULT_CLIPBOARD_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_WIN32_DEFAULT_CLIPBOARD_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_WIN32_DEFAULT_IME_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_WIN32_DEFAULT_IME_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_WIN32_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_WIN32_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_DEFAULT_FORMAT_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_DEFAULT_FORMAT_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_DEFAULT_MATH_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_DEFAULT_MATH_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_DEFAULT_FILE_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_DEFAULT_FILE_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_FILE_FUNCTIONS + ImGui::Text("define: IMGUI_DISABLE_FILE_FUNCTIONS"); +#endif +#ifdef IMGUI_DISABLE_DEFAULT_ALLOCATORS + ImGui::Text("define: IMGUI_DISABLE_DEFAULT_ALLOCATORS"); +#endif +#ifdef IMGUI_USE_BGRA_PACKED_COLOR + ImGui::Text("define: IMGUI_USE_BGRA_PACKED_COLOR"); +#endif +#ifdef _WIN32 + ImGui::Text("define: _WIN32"); +#endif +#ifdef _WIN64 + ImGui::Text("define: _WIN64"); +#endif +#ifdef __linux__ + ImGui::Text("define: __linux__"); +#endif +#ifdef __APPLE__ + ImGui::Text("define: __APPLE__"); +#endif +#ifdef _MSC_VER + ImGui::Text("define: _MSC_VER=%d", _MSC_VER); +#endif +#ifdef _MSVC_LANG + ImGui::Text("define: _MSVC_LANG=%d", (int)_MSVC_LANG); +#endif +#ifdef __MINGW32__ + ImGui::Text("define: __MINGW32__"); +#endif +#ifdef __MINGW64__ + ImGui::Text("define: __MINGW64__"); +#endif +#ifdef __GNUC__ + ImGui::Text("define: __GNUC__=%d", (int)__GNUC__); +#endif +#ifdef __clang_version__ + ImGui::Text("define: __clang_version__=%s", __clang_version__); +#endif +#ifdef __EMSCRIPTEN__ + ImGui::Text("define: __EMSCRIPTEN__"); +#endif +#ifdef IMGUI_HAS_VIEWPORT + ImGui::Text("define: IMGUI_HAS_VIEWPORT"); +#endif +#ifdef IMGUI_HAS_DOCK + ImGui::Text("define: IMGUI_HAS_DOCK"); +#endif + ImGui::Separator(); + ImGui::Text("io.BackendPlatformName: %s", io.BackendPlatformName ? io.BackendPlatformName : "NULL"); + ImGui::Text("io.BackendRendererName: %s", io.BackendRendererName ? io.BackendRendererName : "NULL"); + ImGui::Text("io.ConfigFlags: 0x%08X", io.ConfigFlags); + if (io.ConfigFlags & ImGuiConfigFlags_NavEnableKeyboard) ImGui::Text(" NavEnableKeyboard"); + if (io.ConfigFlags & ImGuiConfigFlags_NavEnableGamepad) ImGui::Text(" NavEnableGamepad"); + if (io.ConfigFlags & ImGuiConfigFlags_NavEnableSetMousePos) ImGui::Text(" NavEnableSetMousePos"); + if (io.ConfigFlags & ImGuiConfigFlags_NavNoCaptureKeyboard) ImGui::Text(" NavNoCaptureKeyboard"); + if (io.ConfigFlags & ImGuiConfigFlags_NoMouse) ImGui::Text(" NoMouse"); + if (io.ConfigFlags & ImGuiConfigFlags_NoMouseCursorChange) ImGui::Text(" NoMouseCursorChange"); + if (io.ConfigFlags & ImGuiConfigFlags_DockingEnable) ImGui::Text(" DockingEnable"); + if (io.ConfigFlags & ImGuiConfigFlags_ViewportsEnable) ImGui::Text(" ViewportsEnable"); + if (io.ConfigFlags & ImGuiConfigFlags_DpiEnableScaleViewports) ImGui::Text(" DpiEnableScaleViewports"); + if (io.ConfigFlags & ImGuiConfigFlags_DpiEnableScaleFonts) ImGui::Text(" DpiEnableScaleFonts"); + if (io.MouseDrawCursor) ImGui::Text("io.MouseDrawCursor"); + if (io.ConfigViewportsNoAutoMerge) ImGui::Text("io.ConfigViewportsNoAutoMerge"); + if (io.ConfigViewportsNoTaskBarIcon) ImGui::Text("io.ConfigViewportsNoTaskBarIcon"); + if (io.ConfigViewportsNoDecoration) ImGui::Text("io.ConfigViewportsNoDecoration"); + if (io.ConfigViewportsNoDefaultParent) ImGui::Text("io.ConfigViewportsNoDefaultParent"); + if (io.ConfigDockingNoSplit) ImGui::Text("io.ConfigDockingNoSplit"); + if (io.ConfigDockingWithShift) ImGui::Text("io.ConfigDockingWithShift"); + if (io.ConfigDockingAlwaysTabBar) ImGui::Text("io.ConfigDockingAlwaysTabBar"); + if (io.ConfigDockingTransparentPayload) ImGui::Text("io.ConfigDockingTransparentPayload"); + if (io.ConfigMacOSXBehaviors) ImGui::Text("io.ConfigMacOSXBehaviors"); + if (io.ConfigInputTextCursorBlink) ImGui::Text("io.ConfigInputTextCursorBlink"); + if (io.ConfigWindowsResizeFromEdges) ImGui::Text("io.ConfigWindowsResizeFromEdges"); + if (io.ConfigWindowsMoveFromTitleBarOnly) ImGui::Text("io.ConfigWindowsMoveFromTitleBarOnly"); + if (io.ConfigMemoryCompactTimer >= 0.0f) ImGui::Text("io.ConfigMemoryCompactTimer = %.1f", io.ConfigMemoryCompactTimer); + ImGui::Text("io.BackendFlags: 0x%08X", io.BackendFlags); + if (io.BackendFlags & ImGuiBackendFlags_HasGamepad) ImGui::Text(" HasGamepad"); + if (io.BackendFlags & ImGuiBackendFlags_HasMouseCursors) ImGui::Text(" HasMouseCursors"); + if (io.BackendFlags & ImGuiBackendFlags_HasSetMousePos) ImGui::Text(" HasSetMousePos"); + if (io.BackendFlags & ImGuiBackendFlags_PlatformHasViewports) ImGui::Text(" PlatformHasViewports"); + if (io.BackendFlags & ImGuiBackendFlags_HasMouseHoveredViewport)ImGui::Text(" HasMouseHoveredViewport"); + if (io.BackendFlags & ImGuiBackendFlags_RendererHasVtxOffset) ImGui::Text(" RendererHasVtxOffset"); + if (io.BackendFlags & ImGuiBackendFlags_RendererHasViewports) ImGui::Text(" RendererHasViewports"); + ImGui::Separator(); + ImGui::Text("io.Fonts: %d fonts, Flags: 0x%08X, TexSize: %d,%d", io.Fonts->Fonts.Size, io.Fonts->Flags, io.Fonts->TexWidth, io.Fonts->TexHeight); + ImGui::Text("io.DisplaySize: %.2f,%.2f", io.DisplaySize.x, io.DisplaySize.y); + ImGui::Text("io.DisplayFramebufferScale: %.2f,%.2f", io.DisplayFramebufferScale.x, io.DisplayFramebufferScale.y); + ImGui::Separator(); + ImGui::Text("style.WindowPadding: %.2f,%.2f", style.WindowPadding.x, style.WindowPadding.y); + ImGui::Text("style.WindowBorderSize: %.2f", style.WindowBorderSize); + ImGui::Text("style.FramePadding: %.2f,%.2f", style.FramePadding.x, style.FramePadding.y); + ImGui::Text("style.FrameRounding: %.2f", style.FrameRounding); + ImGui::Text("style.FrameBorderSize: %.2f", style.FrameBorderSize); + ImGui::Text("style.ItemSpacing: %.2f,%.2f", style.ItemSpacing.x, style.ItemSpacing.y); + ImGui::Text("style.ItemInnerSpacing: %.2f,%.2f", style.ItemInnerSpacing.x, style.ItemInnerSpacing.y); + + if (copy_to_clipboard) + { + ImGui::LogText("\n```\n"); + ImGui::LogFinish(); + } + ImGui::EndChildFrame(); + } + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Style Editor / ShowStyleEditor() +//----------------------------------------------------------------------------- +// - ShowFontSelector() +// - ShowStyleSelector() +// - ShowStyleEditor() +//----------------------------------------------------------------------------- + +// Forward declare ShowFontAtlas() which isn't worth putting in public API yet +namespace ImGui { IMGUI_API void ShowFontAtlas(ImFontAtlas* atlas); } + +// Demo helper function to select among loaded fonts. +// Here we use the regular BeginCombo()/EndCombo() api which is the more flexible one. +void ImGui::ShowFontSelector(const char* label) +{ + ImGuiIO& io = ImGui::GetIO(); + ImFont* font_current = ImGui::GetFont(); + if (ImGui::BeginCombo(label, font_current->GetDebugName())) + { + for (ImFont* font : io.Fonts->Fonts) + { + ImGui::PushID((void*)font); + if (ImGui::Selectable(font->GetDebugName(), font == font_current)) + io.FontDefault = font; + ImGui::PopID(); + } + ImGui::EndCombo(); + } + ImGui::SameLine(); + HelpMarker( + "- Load additional fonts with io.Fonts->AddFontFromFileTTF().\n" + "- The font atlas is built when calling io.Fonts->GetTexDataAsXXXX() or io.Fonts->Build().\n" + "- Read FAQ and docs/FONTS.md for more details.\n" + "- If you need to add/remove fonts at runtime (e.g. for DPI change), do it before calling NewFrame()."); +} + +// Demo helper function to select among default colors. See ShowStyleEditor() for more advanced options. +// Here we use the simplified Combo() api that packs items into a single literal string. +// Useful for quick combo boxes where the choices are known locally. +bool ImGui::ShowStyleSelector(const char* label) +{ + static int style_idx = -1; + if (ImGui::Combo(label, &style_idx, "Dark\0Light\0Classic\0")) + { + switch (style_idx) + { + case 0: ImGui::StyleColorsDark(); break; + case 1: ImGui::StyleColorsLight(); break; + case 2: ImGui::StyleColorsClassic(); break; + } + return true; + } + return false; +} + +void ImGui::ShowStyleEditor(ImGuiStyle* ref) +{ + IMGUI_DEMO_MARKER("Tools/Style Editor"); + // You can pass in a reference ImGuiStyle structure to compare to, revert to and save to + // (without a reference style pointer, we will use one compared locally as a reference) + ImGuiStyle& style = ImGui::GetStyle(); + static ImGuiStyle ref_saved_style; + + // Default to using internal storage as reference + static bool init = true; + if (init && ref == NULL) + ref_saved_style = style; + init = false; + if (ref == NULL) + ref = &ref_saved_style; + + ImGui::PushItemWidth(ImGui::GetWindowWidth() * 0.50f); + + if (ImGui::ShowStyleSelector("Colors##Selector")) + ref_saved_style = style; + ImGui::ShowFontSelector("Fonts##Selector"); + + // Simplified Settings (expose floating-pointer border sizes as boolean representing 0.0f or 1.0f) + if (ImGui::SliderFloat("FrameRounding", &style.FrameRounding, 0.0f, 12.0f, "%.0f")) + style.GrabRounding = style.FrameRounding; // Make GrabRounding always the same value as FrameRounding + { bool border = (style.WindowBorderSize > 0.0f); if (ImGui::Checkbox("WindowBorder", &border)) { style.WindowBorderSize = border ? 1.0f : 0.0f; } } + ImGui::SameLine(); + { bool border = (style.FrameBorderSize > 0.0f); if (ImGui::Checkbox("FrameBorder", &border)) { style.FrameBorderSize = border ? 1.0f : 0.0f; } } + ImGui::SameLine(); + { bool border = (style.PopupBorderSize > 0.0f); if (ImGui::Checkbox("PopupBorder", &border)) { style.PopupBorderSize = border ? 1.0f : 0.0f; } } + + // Save/Revert button + if (ImGui::Button("Save Ref")) + *ref = ref_saved_style = style; + ImGui::SameLine(); + if (ImGui::Button("Revert Ref")) + style = *ref; + ImGui::SameLine(); + HelpMarker( + "Save/Revert in local non-persistent storage. Default Colors definition are not affected. " + "Use \"Export\" below to save them somewhere."); + + ImGui::Separator(); + + if (ImGui::BeginTabBar("##tabs", ImGuiTabBarFlags_None)) + { + if (ImGui::BeginTabItem("Sizes")) + { + ImGui::SeparatorText("Main"); + ImGui::SliderFloat2("WindowPadding", (float*)&style.WindowPadding, 0.0f, 20.0f, "%.0f"); + ImGui::SliderFloat2("FramePadding", (float*)&style.FramePadding, 0.0f, 20.0f, "%.0f"); + ImGui::SliderFloat2("CellPadding", (float*)&style.CellPadding, 0.0f, 20.0f, "%.0f"); + ImGui::SliderFloat2("ItemSpacing", (float*)&style.ItemSpacing, 0.0f, 20.0f, "%.0f"); + ImGui::SliderFloat2("ItemInnerSpacing", (float*)&style.ItemInnerSpacing, 0.0f, 20.0f, "%.0f"); + ImGui::SliderFloat2("TouchExtraPadding", (float*)&style.TouchExtraPadding, 0.0f, 10.0f, "%.0f"); + ImGui::SliderFloat("IndentSpacing", &style.IndentSpacing, 0.0f, 30.0f, "%.0f"); + ImGui::SliderFloat("ScrollbarSize", &style.ScrollbarSize, 1.0f, 20.0f, "%.0f"); + ImGui::SliderFloat("GrabMinSize", &style.GrabMinSize, 1.0f, 20.0f, "%.0f"); + + ImGui::SeparatorText("Borders"); + ImGui::SliderFloat("WindowBorderSize", &style.WindowBorderSize, 0.0f, 1.0f, "%.0f"); + ImGui::SliderFloat("ChildBorderSize", &style.ChildBorderSize, 0.0f, 1.0f, "%.0f"); + ImGui::SliderFloat("PopupBorderSize", &style.PopupBorderSize, 0.0f, 1.0f, "%.0f"); + ImGui::SliderFloat("FrameBorderSize", &style.FrameBorderSize, 0.0f, 1.0f, "%.0f"); + ImGui::SliderFloat("TabBorderSize", &style.TabBorderSize, 0.0f, 1.0f, "%.0f"); + ImGui::SliderFloat("TabBarBorderSize", &style.TabBarBorderSize, 0.0f, 2.0f, "%.0f"); + + ImGui::SeparatorText("Rounding"); + ImGui::SliderFloat("WindowRounding", &style.WindowRounding, 0.0f, 12.0f, "%.0f"); + ImGui::SliderFloat("ChildRounding", &style.ChildRounding, 0.0f, 12.0f, "%.0f"); + ImGui::SliderFloat("FrameRounding", &style.FrameRounding, 0.0f, 12.0f, "%.0f"); + ImGui::SliderFloat("PopupRounding", &style.PopupRounding, 0.0f, 12.0f, "%.0f"); + ImGui::SliderFloat("ScrollbarRounding", &style.ScrollbarRounding, 0.0f, 12.0f, "%.0f"); + ImGui::SliderFloat("GrabRounding", &style.GrabRounding, 0.0f, 12.0f, "%.0f"); + ImGui::SliderFloat("TabRounding", &style.TabRounding, 0.0f, 12.0f, "%.0f"); + + ImGui::SeparatorText("Widgets"); + ImGui::SliderFloat2("WindowTitleAlign", (float*)&style.WindowTitleAlign, 0.0f, 1.0f, "%.2f"); + int window_menu_button_position = style.WindowMenuButtonPosition + 1; + if (ImGui::Combo("WindowMenuButtonPosition", (int*)&window_menu_button_position, "None\0Left\0Right\0")) + style.WindowMenuButtonPosition = window_menu_button_position - 1; + ImGui::Combo("ColorButtonPosition", (int*)&style.ColorButtonPosition, "Left\0Right\0"); + ImGui::SliderFloat2("ButtonTextAlign", (float*)&style.ButtonTextAlign, 0.0f, 1.0f, "%.2f"); + ImGui::SameLine(); HelpMarker("Alignment applies when a button is larger than its text content."); + ImGui::SliderFloat2("SelectableTextAlign", (float*)&style.SelectableTextAlign, 0.0f, 1.0f, "%.2f"); + ImGui::SameLine(); HelpMarker("Alignment applies when a selectable is larger than its text content."); + ImGui::SliderFloat("SeparatorTextBorderSize", &style.SeparatorTextBorderSize, 0.0f, 10.0f, "%.0f"); + ImGui::SliderFloat2("SeparatorTextAlign", (float*)&style.SeparatorTextAlign, 0.0f, 1.0f, "%.2f"); + ImGui::SliderFloat2("SeparatorTextPadding", (float*)&style.SeparatorTextPadding, 0.0f, 40.0f, "%.0f"); + ImGui::SliderFloat("LogSliderDeadzone", &style.LogSliderDeadzone, 0.0f, 12.0f, "%.0f"); + + ImGui::SeparatorText("Docking"); + ImGui::SliderFloat("DockingSplitterSize", &style.DockingSeparatorSize, 0.0f, 12.0f, "%.0f"); + + ImGui::SeparatorText("Tooltips"); + for (int n = 0; n < 2; n++) + if (ImGui::TreeNodeEx(n == 0 ? "HoverFlagsForTooltipMouse" : "HoverFlagsForTooltipNav")) + { + ImGuiHoveredFlags* p = (n == 0) ? &style.HoverFlagsForTooltipMouse : &style.HoverFlagsForTooltipNav; + ImGui::CheckboxFlags("ImGuiHoveredFlags_DelayNone", p, ImGuiHoveredFlags_DelayNone); + ImGui::CheckboxFlags("ImGuiHoveredFlags_DelayShort", p, ImGuiHoveredFlags_DelayShort); + ImGui::CheckboxFlags("ImGuiHoveredFlags_DelayNormal", p, ImGuiHoveredFlags_DelayNormal); + ImGui::CheckboxFlags("ImGuiHoveredFlags_Stationary", p, ImGuiHoveredFlags_Stationary); + ImGui::CheckboxFlags("ImGuiHoveredFlags_NoSharedDelay", p, ImGuiHoveredFlags_NoSharedDelay); + ImGui::TreePop(); + } + + ImGui::SeparatorText("Misc"); + ImGui::SliderFloat2("DisplaySafeAreaPadding", (float*)&style.DisplaySafeAreaPadding, 0.0f, 30.0f, "%.0f"); ImGui::SameLine(); HelpMarker("Adjust if you cannot see the edges of your screen (e.g. on a TV where scaling has not been configured)."); + + ImGui::EndTabItem(); + } + + if (ImGui::BeginTabItem("Colors")) + { + static int output_dest = 0; + static bool output_only_modified = true; + if (ImGui::Button("Export")) + { + if (output_dest == 0) + ImGui::LogToClipboard(); + else + ImGui::LogToTTY(); + ImGui::LogText("ImVec4* colors = ImGui::GetStyle().Colors;" IM_NEWLINE); + for (int i = 0; i < ImGuiCol_COUNT; i++) + { + const ImVec4& col = style.Colors[i]; + const char* name = ImGui::GetStyleColorName(i); + if (!output_only_modified || memcmp(&col, &ref->Colors[i], sizeof(ImVec4)) != 0) + ImGui::LogText("colors[ImGuiCol_%s]%*s= ImVec4(%.2ff, %.2ff, %.2ff, %.2ff);" IM_NEWLINE, + name, 23 - (int)strlen(name), "", col.x, col.y, col.z, col.w); + } + ImGui::LogFinish(); + } + ImGui::SameLine(); ImGui::SetNextItemWidth(120); ImGui::Combo("##output_type", &output_dest, "To Clipboard\0To TTY\0"); + ImGui::SameLine(); ImGui::Checkbox("Only Modified Colors", &output_only_modified); + + static ImGuiTextFilter filter; + filter.Draw("Filter colors", ImGui::GetFontSize() * 16); + + static ImGuiColorEditFlags alpha_flags = 0; + if (ImGui::RadioButton("Opaque", alpha_flags == ImGuiColorEditFlags_None)) { alpha_flags = ImGuiColorEditFlags_None; } ImGui::SameLine(); + if (ImGui::RadioButton("Alpha", alpha_flags == ImGuiColorEditFlags_AlphaPreview)) { alpha_flags = ImGuiColorEditFlags_AlphaPreview; } ImGui::SameLine(); + if (ImGui::RadioButton("Both", alpha_flags == ImGuiColorEditFlags_AlphaPreviewHalf)) { alpha_flags = ImGuiColorEditFlags_AlphaPreviewHalf; } ImGui::SameLine(); + HelpMarker( + "In the color list:\n" + "Left-click on color square to open color picker,\n" + "Right-click to open edit options menu."); + + ImGui::BeginChild("##colors", ImVec2(0, 0), true, ImGuiWindowFlags_AlwaysVerticalScrollbar | ImGuiWindowFlags_AlwaysHorizontalScrollbar | ImGuiWindowFlags_NavFlattened); + ImGui::PushItemWidth(-160); + for (int i = 0; i < ImGuiCol_COUNT; i++) + { + const char* name = ImGui::GetStyleColorName(i); + if (!filter.PassFilter(name)) + continue; + ImGui::PushID(i); + ImGui::ColorEdit4("##color", (float*)&style.Colors[i], ImGuiColorEditFlags_AlphaBar | alpha_flags); + if (memcmp(&style.Colors[i], &ref->Colors[i], sizeof(ImVec4)) != 0) + { + // Tips: in a real user application, you may want to merge and use an icon font into the main font, + // so instead of "Save"/"Revert" you'd use icons! + // Read the FAQ and docs/FONTS.md about using icon fonts. It's really easy and super convenient! + ImGui::SameLine(0.0f, style.ItemInnerSpacing.x); if (ImGui::Button("Save")) { ref->Colors[i] = style.Colors[i]; } + ImGui::SameLine(0.0f, style.ItemInnerSpacing.x); if (ImGui::Button("Revert")) { style.Colors[i] = ref->Colors[i]; } + } + ImGui::SameLine(0.0f, style.ItemInnerSpacing.x); + ImGui::TextUnformatted(name); + ImGui::PopID(); + } + ImGui::PopItemWidth(); + ImGui::EndChild(); + + ImGui::EndTabItem(); + } + + if (ImGui::BeginTabItem("Fonts")) + { + ImGuiIO& io = ImGui::GetIO(); + ImFontAtlas* atlas = io.Fonts; + HelpMarker("Read FAQ and docs/FONTS.md for details on font loading."); + ImGui::ShowFontAtlas(atlas); + + // Post-baking font scaling. Note that this is NOT the nice way of scaling fonts, read below. + // (we enforce hard clamping manually as by default DragFloat/SliderFloat allows CTRL+Click text to get out of bounds). + const float MIN_SCALE = 0.3f; + const float MAX_SCALE = 2.0f; + HelpMarker( + "Those are old settings provided for convenience.\n" + "However, the _correct_ way of scaling your UI is currently to reload your font at the designed size, " + "rebuild the font atlas, and call style.ScaleAllSizes() on a reference ImGuiStyle structure.\n" + "Using those settings here will give you poor quality results."); + static float window_scale = 1.0f; + ImGui::PushItemWidth(ImGui::GetFontSize() * 8); + if (ImGui::DragFloat("window scale", &window_scale, 0.005f, MIN_SCALE, MAX_SCALE, "%.2f", ImGuiSliderFlags_AlwaysClamp)) // Scale only this window + ImGui::SetWindowFontScale(window_scale); + ImGui::DragFloat("global scale", &io.FontGlobalScale, 0.005f, MIN_SCALE, MAX_SCALE, "%.2f", ImGuiSliderFlags_AlwaysClamp); // Scale everything + ImGui::PopItemWidth(); + + ImGui::EndTabItem(); + } + + if (ImGui::BeginTabItem("Rendering")) + { + ImGui::Checkbox("Anti-aliased lines", &style.AntiAliasedLines); + ImGui::SameLine(); + HelpMarker("When disabling anti-aliasing lines, you'll probably want to disable borders in your style as well."); + + ImGui::Checkbox("Anti-aliased lines use texture", &style.AntiAliasedLinesUseTex); + ImGui::SameLine(); + HelpMarker("Faster lines using texture data. Require backend to render with bilinear filtering (not point/nearest filtering)."); + + ImGui::Checkbox("Anti-aliased fill", &style.AntiAliasedFill); + ImGui::PushItemWidth(ImGui::GetFontSize() * 8); + ImGui::DragFloat("Curve Tessellation Tolerance", &style.CurveTessellationTol, 0.02f, 0.10f, 10.0f, "%.2f"); + if (style.CurveTessellationTol < 0.10f) style.CurveTessellationTol = 0.10f; + + // When editing the "Circle Segment Max Error" value, draw a preview of its effect on auto-tessellated circles. + ImGui::DragFloat("Circle Tessellation Max Error", &style.CircleTessellationMaxError , 0.005f, 0.10f, 5.0f, "%.2f", ImGuiSliderFlags_AlwaysClamp); + const bool show_samples = ImGui::IsItemActive(); + if (show_samples) + ImGui::SetNextWindowPos(ImGui::GetCursorScreenPos()); + if (show_samples && ImGui::BeginTooltip()) + { + ImGui::TextUnformatted("(R = radius, N = number of segments)"); + ImGui::Spacing(); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + const float min_widget_width = ImGui::CalcTextSize("N: MMM\nR: MMM").x; + for (int n = 0; n < 8; n++) + { + const float RAD_MIN = 5.0f; + const float RAD_MAX = 70.0f; + const float rad = RAD_MIN + (RAD_MAX - RAD_MIN) * (float)n / (8.0f - 1.0f); + + ImGui::BeginGroup(); + + ImGui::Text("R: %.f\nN: %d", rad, draw_list->_CalcCircleAutoSegmentCount(rad)); + + const float canvas_width = IM_MAX(min_widget_width, rad * 2.0f); + const float offset_x = floorf(canvas_width * 0.5f); + const float offset_y = floorf(RAD_MAX); + + const ImVec2 p1 = ImGui::GetCursorScreenPos(); + draw_list->AddCircle(ImVec2(p1.x + offset_x, p1.y + offset_y), rad, ImGui::GetColorU32(ImGuiCol_Text)); + ImGui::Dummy(ImVec2(canvas_width, RAD_MAX * 2)); + + /* + const ImVec2 p2 = ImGui::GetCursorScreenPos(); + draw_list->AddCircleFilled(ImVec2(p2.x + offset_x, p2.y + offset_y), rad, ImGui::GetColorU32(ImGuiCol_Text)); + ImGui::Dummy(ImVec2(canvas_width, RAD_MAX * 2)); + */ + + ImGui::EndGroup(); + ImGui::SameLine(); + } + ImGui::EndTooltip(); + } + ImGui::SameLine(); + HelpMarker("When drawing circle primitives with \"num_segments == 0\" tesselation will be calculated automatically."); + + ImGui::DragFloat("Global Alpha", &style.Alpha, 0.005f, 0.20f, 1.0f, "%.2f"); // Not exposing zero here so user doesn't "lose" the UI (zero alpha clips all widgets). But application code could have a toggle to switch between zero and non-zero. + ImGui::DragFloat("Disabled Alpha", &style.DisabledAlpha, 0.005f, 0.0f, 1.0f, "%.2f"); ImGui::SameLine(); HelpMarker("Additional alpha multiplier for disabled items (multiply over current value of Alpha)."); + ImGui::PopItemWidth(); + + ImGui::EndTabItem(); + } + + ImGui::EndTabBar(); + } + + ImGui::PopItemWidth(); +} + +//----------------------------------------------------------------------------- +// [SECTION] User Guide / ShowUserGuide() +//----------------------------------------------------------------------------- + +void ImGui::ShowUserGuide() +{ + ImGuiIO& io = ImGui::GetIO(); + ImGui::BulletText("Double-click on title bar to collapse window."); + ImGui::BulletText( + "Click and drag on lower corner to resize window\n" + "(double-click to auto fit window to its contents)."); + ImGui::BulletText("CTRL+Click on a slider or drag box to input value as text."); + ImGui::BulletText("TAB/SHIFT+TAB to cycle through keyboard editable fields."); + ImGui::BulletText("CTRL+Tab to select a window."); + if (io.FontAllowUserScaling) + ImGui::BulletText("CTRL+Mouse Wheel to zoom window contents."); + ImGui::BulletText("While inputing text:\n"); + ImGui::Indent(); + ImGui::BulletText("CTRL+Left/Right to word jump."); + ImGui::BulletText("CTRL+A or double-click to select all."); + ImGui::BulletText("CTRL+X/C/V to use clipboard cut/copy/paste."); + ImGui::BulletText("CTRL+Z,CTRL+Y to undo/redo."); + ImGui::BulletText("ESCAPE to revert."); + ImGui::Unindent(); + ImGui::BulletText("With keyboard navigation enabled:"); + ImGui::Indent(); + ImGui::BulletText("Arrow keys to navigate."); + ImGui::BulletText("Space to activate a widget."); + ImGui::BulletText("Return to input text into a widget."); + ImGui::BulletText("Escape to deactivate a widget, close popup, exit child window."); + ImGui::BulletText("Alt to jump to the menu layer of a window."); + ImGui::Unindent(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Main Menu Bar / ShowExampleAppMainMenuBar() +//----------------------------------------------------------------------------- +// - ShowExampleAppMainMenuBar() +// - ShowExampleMenuFile() +//----------------------------------------------------------------------------- + +// Demonstrate creating a "main" fullscreen menu bar and populating it. +// Note the difference between BeginMainMenuBar() and BeginMenuBar(): +// - BeginMenuBar() = menu-bar inside current window (which needs the ImGuiWindowFlags_MenuBar flag!) +// - BeginMainMenuBar() = helper to create menu-bar-sized window at the top of the main viewport + call BeginMenuBar() into it. +static void ShowExampleAppMainMenuBar() +{ + if (ImGui::BeginMainMenuBar()) + { + if (ImGui::BeginMenu("File")) + { + ShowExampleMenuFile(); + ImGui::EndMenu(); + } + if (ImGui::BeginMenu("Edit")) + { + if (ImGui::MenuItem("Undo", "CTRL+Z")) {} + if (ImGui::MenuItem("Redo", "CTRL+Y", false, false)) {} // Disabled item + ImGui::Separator(); + if (ImGui::MenuItem("Cut", "CTRL+X")) {} + if (ImGui::MenuItem("Copy", "CTRL+C")) {} + if (ImGui::MenuItem("Paste", "CTRL+V")) {} + ImGui::EndMenu(); + } + ImGui::EndMainMenuBar(); + } +} + +// Note that shortcuts are currently provided for display only +// (future version will add explicit flags to BeginMenu() to request processing shortcuts) +static void ShowExampleMenuFile() +{ + IMGUI_DEMO_MARKER("Examples/Menu"); + ImGui::MenuItem("(demo menu)", NULL, false, false); + if (ImGui::MenuItem("New")) {} + if (ImGui::MenuItem("Open", "Ctrl+O")) {} + if (ImGui::BeginMenu("Open Recent")) + { + ImGui::MenuItem("fish_hat.c"); + ImGui::MenuItem("fish_hat.inl"); + ImGui::MenuItem("fish_hat.h"); + if (ImGui::BeginMenu("More..")) + { + ImGui::MenuItem("Hello"); + ImGui::MenuItem("Sailor"); + if (ImGui::BeginMenu("Recurse..")) + { + ShowExampleMenuFile(); + ImGui::EndMenu(); + } + ImGui::EndMenu(); + } + ImGui::EndMenu(); + } + if (ImGui::MenuItem("Save", "Ctrl+S")) {} + if (ImGui::MenuItem("Save As..")) {} + + ImGui::Separator(); + IMGUI_DEMO_MARKER("Examples/Menu/Options"); + if (ImGui::BeginMenu("Options")) + { + static bool enabled = true; + ImGui::MenuItem("Enabled", "", &enabled); + ImGui::BeginChild("child", ImVec2(0, 60), true); + for (int i = 0; i < 10; i++) + ImGui::Text("Scrolling Text %d", i); + ImGui::EndChild(); + static float f = 0.5f; + static int n = 0; + ImGui::SliderFloat("Value", &f, 0.0f, 1.0f); + ImGui::InputFloat("Input", &f, 0.1f); + ImGui::Combo("Combo", &n, "Yes\0No\0Maybe\0\0"); + ImGui::EndMenu(); + } + + IMGUI_DEMO_MARKER("Examples/Menu/Colors"); + if (ImGui::BeginMenu("Colors")) + { + float sz = ImGui::GetTextLineHeight(); + for (int i = 0; i < ImGuiCol_COUNT; i++) + { + const char* name = ImGui::GetStyleColorName((ImGuiCol)i); + ImVec2 p = ImGui::GetCursorScreenPos(); + ImGui::GetWindowDrawList()->AddRectFilled(p, ImVec2(p.x + sz, p.y + sz), ImGui::GetColorU32((ImGuiCol)i)); + ImGui::Dummy(ImVec2(sz, sz)); + ImGui::SameLine(); + ImGui::MenuItem(name); + } + ImGui::EndMenu(); + } + + // Here we demonstrate appending again to the "Options" menu (which we already created above) + // Of course in this demo it is a little bit silly that this function calls BeginMenu("Options") twice. + // In a real code-base using it would make senses to use this feature from very different code locations. + if (ImGui::BeginMenu("Options")) // <-- Append! + { + IMGUI_DEMO_MARKER("Examples/Menu/Append to an existing menu"); + static bool b = true; + ImGui::Checkbox("SomeOption", &b); + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Disabled", false)) // Disabled + { + IM_ASSERT(0); + } + if (ImGui::MenuItem("Checked", NULL, true)) {} + ImGui::Separator(); + if (ImGui::MenuItem("Quit", "Alt+F4")) {} +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Debug Console / ShowExampleAppConsole() +//----------------------------------------------------------------------------- + +// Demonstrate creating a simple console window, with scrolling, filtering, completion and history. +// For the console example, we are using a more C++ like approach of declaring a class to hold both data and functions. +struct ExampleAppConsole +{ + char InputBuf[256]; + ImVector Items; + ImVector Commands; + ImVector History; + int HistoryPos; // -1: new line, 0..History.Size-1 browsing history. + ImGuiTextFilter Filter; + bool AutoScroll; + bool ScrollToBottom; + + ExampleAppConsole() + { + IMGUI_DEMO_MARKER("Examples/Console"); + ClearLog(); + memset(InputBuf, 0, sizeof(InputBuf)); + HistoryPos = -1; + + // "CLASSIFY" is here to provide the test case where "C"+[tab] completes to "CL" and display multiple matches. + Commands.push_back("HELP"); + Commands.push_back("HISTORY"); + Commands.push_back("CLEAR"); + Commands.push_back("CLASSIFY"); + AutoScroll = true; + ScrollToBottom = false; + AddLog("Welcome to Dear ImGui!"); + } + ~ExampleAppConsole() + { + ClearLog(); + for (int i = 0; i < History.Size; i++) + free(History[i]); + } + + // Portable helpers + static int Stricmp(const char* s1, const char* s2) { int d; while ((d = toupper(*s2) - toupper(*s1)) == 0 && *s1) { s1++; s2++; } return d; } + static int Strnicmp(const char* s1, const char* s2, int n) { int d = 0; while (n > 0 && (d = toupper(*s2) - toupper(*s1)) == 0 && *s1) { s1++; s2++; n--; } return d; } + static char* Strdup(const char* s) { IM_ASSERT(s); size_t len = strlen(s) + 1; void* buf = malloc(len); IM_ASSERT(buf); return (char*)memcpy(buf, (const void*)s, len); } + static void Strtrim(char* s) { char* str_end = s + strlen(s); while (str_end > s && str_end[-1] == ' ') str_end--; *str_end = 0; } + + void ClearLog() + { + for (int i = 0; i < Items.Size; i++) + free(Items[i]); + Items.clear(); + } + + void AddLog(const char* fmt, ...) IM_FMTARGS(2) + { + // FIXME-OPT + char buf[1024]; + va_list args; + va_start(args, fmt); + vsnprintf(buf, IM_ARRAYSIZE(buf), fmt, args); + buf[IM_ARRAYSIZE(buf)-1] = 0; + va_end(args); + Items.push_back(Strdup(buf)); + } + + void Draw(const char* title, bool* p_open) + { + ImGui::SetNextWindowSize(ImVec2(520, 600), ImGuiCond_FirstUseEver); + if (!ImGui::Begin(title, p_open)) + { + ImGui::End(); + return; + } + + // As a specific feature guaranteed by the library, after calling Begin() the last Item represent the title bar. + // So e.g. IsItemHovered() will return true when hovering the title bar. + // Here we create a context menu only available from the title bar. + if (ImGui::BeginPopupContextItem()) + { + if (ImGui::MenuItem("Close Console")) + *p_open = false; + ImGui::EndPopup(); + } + + ImGui::TextWrapped( + "This example implements a console with basic coloring, completion (TAB key) and history (Up/Down keys). A more elaborate " + "implementation may want to store entries along with extra data such as timestamp, emitter, etc."); + ImGui::TextWrapped("Enter 'HELP' for help."); + + // TODO: display items starting from the bottom + + if (ImGui::SmallButton("Add Debug Text")) { AddLog("%d some text", Items.Size); AddLog("some more text"); AddLog("display very important message here!"); } + ImGui::SameLine(); + if (ImGui::SmallButton("Add Debug Error")) { AddLog("[error] something went wrong"); } + ImGui::SameLine(); + if (ImGui::SmallButton("Clear")) { ClearLog(); } + ImGui::SameLine(); + bool copy_to_clipboard = ImGui::SmallButton("Copy"); + //static float t = 0.0f; if (ImGui::GetTime() - t > 0.02f) { t = ImGui::GetTime(); AddLog("Spam %f", t); } + + ImGui::Separator(); + + // Options menu + if (ImGui::BeginPopup("Options")) + { + ImGui::Checkbox("Auto-scroll", &AutoScroll); + ImGui::EndPopup(); + } + + // Options, Filter + if (ImGui::Button("Options")) + ImGui::OpenPopup("Options"); + ImGui::SameLine(); + Filter.Draw("Filter (\"incl,-excl\") (\"error\")", 180); + ImGui::Separator(); + + // Reserve enough left-over height for 1 separator + 1 input text + const float footer_height_to_reserve = ImGui::GetStyle().ItemSpacing.y + ImGui::GetFrameHeightWithSpacing(); + if (ImGui::BeginChild("ScrollingRegion", ImVec2(0, -footer_height_to_reserve), false, ImGuiWindowFlags_HorizontalScrollbar)) + { + if (ImGui::BeginPopupContextWindow()) + { + if (ImGui::Selectable("Clear")) ClearLog(); + ImGui::EndPopup(); + } + + // Display every line as a separate entry so we can change their color or add custom widgets. + // If you only want raw text you can use ImGui::TextUnformatted(log.begin(), log.end()); + // NB- if you have thousands of entries this approach may be too inefficient and may require user-side clipping + // to only process visible items. The clipper will automatically measure the height of your first item and then + // "seek" to display only items in the visible area. + // To use the clipper we can replace your standard loop: + // for (int i = 0; i < Items.Size; i++) + // With: + // ImGuiListClipper clipper; + // clipper.Begin(Items.Size); + // while (clipper.Step()) + // for (int i = clipper.DisplayStart; i < clipper.DisplayEnd; i++) + // - That your items are evenly spaced (same height) + // - That you have cheap random access to your elements (you can access them given their index, + // without processing all the ones before) + // You cannot this code as-is if a filter is active because it breaks the 'cheap random-access' property. + // We would need random-access on the post-filtered list. + // A typical application wanting coarse clipping and filtering may want to pre-compute an array of indices + // or offsets of items that passed the filtering test, recomputing this array when user changes the filter, + // and appending newly elements as they are inserted. This is left as a task to the user until we can manage + // to improve this example code! + // If your items are of variable height: + // - Split them into same height items would be simpler and facilitate random-seeking into your list. + // - Consider using manual call to IsRectVisible() and skipping extraneous decoration from your items. + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(4, 1)); // Tighten spacing + if (copy_to_clipboard) + ImGui::LogToClipboard(); + for (const char* item : Items) + { + if (!Filter.PassFilter(item)) + continue; + + // Normally you would store more information in your item than just a string. + // (e.g. make Items[] an array of structure, store color/type etc.) + ImVec4 color; + bool has_color = false; + if (strstr(item, "[error]")) { color = ImVec4(1.0f, 0.4f, 0.4f, 1.0f); has_color = true; } + else if (strncmp(item, "# ", 2) == 0) { color = ImVec4(1.0f, 0.8f, 0.6f, 1.0f); has_color = true; } + if (has_color) + ImGui::PushStyleColor(ImGuiCol_Text, color); + ImGui::TextUnformatted(item); + if (has_color) + ImGui::PopStyleColor(); + } + if (copy_to_clipboard) + ImGui::LogFinish(); + + // Keep up at the bottom of the scroll region if we were already at the bottom at the beginning of the frame. + // Using a scrollbar or mouse-wheel will take away from the bottom edge. + if (ScrollToBottom || (AutoScroll && ImGui::GetScrollY() >= ImGui::GetScrollMaxY())) + ImGui::SetScrollHereY(1.0f); + ScrollToBottom = false; + + ImGui::PopStyleVar(); + } + ImGui::EndChild(); + ImGui::Separator(); + + // Command-line + bool reclaim_focus = false; + ImGuiInputTextFlags input_text_flags = ImGuiInputTextFlags_EnterReturnsTrue | ImGuiInputTextFlags_EscapeClearsAll | ImGuiInputTextFlags_CallbackCompletion | ImGuiInputTextFlags_CallbackHistory; + if (ImGui::InputText("Input", InputBuf, IM_ARRAYSIZE(InputBuf), input_text_flags, &TextEditCallbackStub, (void*)this)) + { + char* s = InputBuf; + Strtrim(s); + if (s[0]) + ExecCommand(s); + strcpy(s, ""); + reclaim_focus = true; + } + + // Auto-focus on window apparition + ImGui::SetItemDefaultFocus(); + if (reclaim_focus) + ImGui::SetKeyboardFocusHere(-1); // Auto focus previous widget + + ImGui::End(); + } + + void ExecCommand(const char* command_line) + { + AddLog("# %s\n", command_line); + + // Insert into history. First find match and delete it so it can be pushed to the back. + // This isn't trying to be smart or optimal. + HistoryPos = -1; + for (int i = History.Size - 1; i >= 0; i--) + if (Stricmp(History[i], command_line) == 0) + { + free(History[i]); + History.erase(History.begin() + i); + break; + } + History.push_back(Strdup(command_line)); + + // Process command + if (Stricmp(command_line, "CLEAR") == 0) + { + ClearLog(); + } + else if (Stricmp(command_line, "HELP") == 0) + { + AddLog("Commands:"); + for (int i = 0; i < Commands.Size; i++) + AddLog("- %s", Commands[i]); + } + else if (Stricmp(command_line, "HISTORY") == 0) + { + int first = History.Size - 10; + for (int i = first > 0 ? first : 0; i < History.Size; i++) + AddLog("%3d: %s\n", i, History[i]); + } + else + { + AddLog("Unknown command: '%s'\n", command_line); + } + + // On command input, we scroll to bottom even if AutoScroll==false + ScrollToBottom = true; + } + + // In C++11 you'd be better off using lambdas for this sort of forwarding callbacks + static int TextEditCallbackStub(ImGuiInputTextCallbackData* data) + { + ExampleAppConsole* console = (ExampleAppConsole*)data->UserData; + return console->TextEditCallback(data); + } + + int TextEditCallback(ImGuiInputTextCallbackData* data) + { + //AddLog("cursor: %d, selection: %d-%d", data->CursorPos, data->SelectionStart, data->SelectionEnd); + switch (data->EventFlag) + { + case ImGuiInputTextFlags_CallbackCompletion: + { + // Example of TEXT COMPLETION + + // Locate beginning of current word + const char* word_end = data->Buf + data->CursorPos; + const char* word_start = word_end; + while (word_start > data->Buf) + { + const char c = word_start[-1]; + if (c == ' ' || c == '\t' || c == ',' || c == ';') + break; + word_start--; + } + + // Build a list of candidates + ImVector candidates; + for (int i = 0; i < Commands.Size; i++) + if (Strnicmp(Commands[i], word_start, (int)(word_end - word_start)) == 0) + candidates.push_back(Commands[i]); + + if (candidates.Size == 0) + { + // No match + AddLog("No match for \"%.*s\"!\n", (int)(word_end - word_start), word_start); + } + else if (candidates.Size == 1) + { + // Single match. Delete the beginning of the word and replace it entirely so we've got nice casing. + data->DeleteChars((int)(word_start - data->Buf), (int)(word_end - word_start)); + data->InsertChars(data->CursorPos, candidates[0]); + data->InsertChars(data->CursorPos, " "); + } + else + { + // Multiple matches. Complete as much as we can.. + // So inputing "C"+Tab will complete to "CL" then display "CLEAR" and "CLASSIFY" as matches. + int match_len = (int)(word_end - word_start); + for (;;) + { + int c = 0; + bool all_candidates_matches = true; + for (int i = 0; i < candidates.Size && all_candidates_matches; i++) + if (i == 0) + c = toupper(candidates[i][match_len]); + else if (c == 0 || c != toupper(candidates[i][match_len])) + all_candidates_matches = false; + if (!all_candidates_matches) + break; + match_len++; + } + + if (match_len > 0) + { + data->DeleteChars((int)(word_start - data->Buf), (int)(word_end - word_start)); + data->InsertChars(data->CursorPos, candidates[0], candidates[0] + match_len); + } + + // List matches + AddLog("Possible matches:\n"); + for (int i = 0; i < candidates.Size; i++) + AddLog("- %s\n", candidates[i]); + } + + break; + } + case ImGuiInputTextFlags_CallbackHistory: + { + // Example of HISTORY + const int prev_history_pos = HistoryPos; + if (data->EventKey == ImGuiKey_UpArrow) + { + if (HistoryPos == -1) + HistoryPos = History.Size - 1; + else if (HistoryPos > 0) + HistoryPos--; + } + else if (data->EventKey == ImGuiKey_DownArrow) + { + if (HistoryPos != -1) + if (++HistoryPos >= History.Size) + HistoryPos = -1; + } + + // A better implementation would preserve the data on the current input line along with cursor position. + if (prev_history_pos != HistoryPos) + { + const char* history_str = (HistoryPos >= 0) ? History[HistoryPos] : ""; + data->DeleteChars(0, data->BufTextLen); + data->InsertChars(0, history_str); + } + } + } + return 0; + } +}; + +static void ShowExampleAppConsole(bool* p_open) +{ + static ExampleAppConsole console; + console.Draw("Example: Console", p_open); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Debug Log / ShowExampleAppLog() +//----------------------------------------------------------------------------- + +// Usage: +// static ExampleAppLog my_log; +// my_log.AddLog("Hello %d world\n", 123); +// my_log.Draw("title"); +struct ExampleAppLog +{ + ImGuiTextBuffer Buf; + ImGuiTextFilter Filter; + ImVector LineOffsets; // Index to lines offset. We maintain this with AddLog() calls. + bool AutoScroll; // Keep scrolling if already at the bottom. + + ExampleAppLog() + { + AutoScroll = true; + Clear(); + } + + void Clear() + { + Buf.clear(); + LineOffsets.clear(); + LineOffsets.push_back(0); + } + + void AddLog(const char* fmt, ...) IM_FMTARGS(2) + { + int old_size = Buf.size(); + va_list args; + va_start(args, fmt); + Buf.appendfv(fmt, args); + va_end(args); + for (int new_size = Buf.size(); old_size < new_size; old_size++) + if (Buf[old_size] == '\n') + LineOffsets.push_back(old_size + 1); + } + + void Draw(const char* title, bool* p_open = NULL) + { + if (!ImGui::Begin(title, p_open)) + { + ImGui::End(); + return; + } + + // Options menu + if (ImGui::BeginPopup("Options")) + { + ImGui::Checkbox("Auto-scroll", &AutoScroll); + ImGui::EndPopup(); + } + + // Main window + if (ImGui::Button("Options")) + ImGui::OpenPopup("Options"); + ImGui::SameLine(); + bool clear = ImGui::Button("Clear"); + ImGui::SameLine(); + bool copy = ImGui::Button("Copy"); + ImGui::SameLine(); + Filter.Draw("Filter", -100.0f); + + ImGui::Separator(); + + if (ImGui::BeginChild("scrolling", ImVec2(0, 0), false, ImGuiWindowFlags_HorizontalScrollbar)) + { + if (clear) + Clear(); + if (copy) + ImGui::LogToClipboard(); + + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(0, 0)); + const char* buf = Buf.begin(); + const char* buf_end = Buf.end(); + if (Filter.IsActive()) + { + // In this example we don't use the clipper when Filter is enabled. + // This is because we don't have random access to the result of our filter. + // A real application processing logs with ten of thousands of entries may want to store the result of + // search/filter.. especially if the filtering function is not trivial (e.g. reg-exp). + for (int line_no = 0; line_no < LineOffsets.Size; line_no++) + { + const char* line_start = buf + LineOffsets[line_no]; + const char* line_end = (line_no + 1 < LineOffsets.Size) ? (buf + LineOffsets[line_no + 1] - 1) : buf_end; + if (Filter.PassFilter(line_start, line_end)) + ImGui::TextUnformatted(line_start, line_end); + } + } + else + { + // The simplest and easy way to display the entire buffer: + // ImGui::TextUnformatted(buf_begin, buf_end); + // And it'll just work. TextUnformatted() has specialization for large blob of text and will fast-forward + // to skip non-visible lines. Here we instead demonstrate using the clipper to only process lines that are + // within the visible area. + // If you have tens of thousands of items and their processing cost is non-negligible, coarse clipping them + // on your side is recommended. Using ImGuiListClipper requires + // - A) random access into your data + // - B) items all being the same height, + // both of which we can handle since we have an array pointing to the beginning of each line of text. + // When using the filter (in the block of code above) we don't have random access into the data to display + // anymore, which is why we don't use the clipper. Storing or skimming through the search result would make + // it possible (and would be recommended if you want to search through tens of thousands of entries). + ImGuiListClipper clipper; + clipper.Begin(LineOffsets.Size); + while (clipper.Step()) + { + for (int line_no = clipper.DisplayStart; line_no < clipper.DisplayEnd; line_no++) + { + const char* line_start = buf + LineOffsets[line_no]; + const char* line_end = (line_no + 1 < LineOffsets.Size) ? (buf + LineOffsets[line_no + 1] - 1) : buf_end; + ImGui::TextUnformatted(line_start, line_end); + } + } + clipper.End(); + } + ImGui::PopStyleVar(); + + // Keep up at the bottom of the scroll region if we were already at the bottom at the beginning of the frame. + // Using a scrollbar or mouse-wheel will take away from the bottom edge. + if (AutoScroll && ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) + ImGui::SetScrollHereY(1.0f); + } + ImGui::EndChild(); + ImGui::End(); + } +}; + +// Demonstrate creating a simple log window with basic filtering. +static void ShowExampleAppLog(bool* p_open) +{ + static ExampleAppLog log; + + // For the demo: add a debug button _BEFORE_ the normal log window contents + // We take advantage of a rarely used feature: multiple calls to Begin()/End() are appending to the _same_ window. + // Most of the contents of the window will be added by the log.Draw() call. + ImGui::SetNextWindowSize(ImVec2(500, 400), ImGuiCond_FirstUseEver); + ImGui::Begin("Example: Log", p_open); + IMGUI_DEMO_MARKER("Examples/Log"); + if (ImGui::SmallButton("[Debug] Add 5 entries")) + { + static int counter = 0; + const char* categories[3] = { "info", "warn", "error" }; + const char* words[] = { "Bumfuzzled", "Cattywampus", "Snickersnee", "Abibliophobia", "Absquatulate", "Nincompoop", "Pauciloquent" }; + for (int n = 0; n < 5; n++) + { + const char* category = categories[counter % IM_ARRAYSIZE(categories)]; + const char* word = words[counter % IM_ARRAYSIZE(words)]; + log.AddLog("[%05d] [%s] Hello, current time is %.1f, here's a word: '%s'\n", + ImGui::GetFrameCount(), category, ImGui::GetTime(), word); + counter++; + } + } + ImGui::End(); + + // Actually call in the regular Log helper (which will Begin() into the same window as we just did) + log.Draw("Example: Log", p_open); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Simple Layout / ShowExampleAppLayout() +//----------------------------------------------------------------------------- + +// Demonstrate create a window with multiple child windows. +static void ShowExampleAppLayout(bool* p_open) +{ + ImGui::SetNextWindowSize(ImVec2(500, 440), ImGuiCond_FirstUseEver); + if (ImGui::Begin("Example: Simple layout", p_open, ImGuiWindowFlags_MenuBar)) + { + IMGUI_DEMO_MARKER("Examples/Simple layout"); + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("File")) + { + if (ImGui::MenuItem("Close", "Ctrl+W")) { *p_open = false; } + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + + // Left + static int selected = 0; + { + ImGui::BeginChild("left pane", ImVec2(150, 0), true); + for (int i = 0; i < 100; i++) + { + // FIXME: Good candidate to use ImGuiSelectableFlags_SelectOnNav + char label[128]; + sprintf(label, "MyObject %d", i); + if (ImGui::Selectable(label, selected == i)) + selected = i; + } + ImGui::EndChild(); + } + ImGui::SameLine(); + + // Right + { + ImGui::BeginGroup(); + ImGui::BeginChild("item view", ImVec2(0, -ImGui::GetFrameHeightWithSpacing())); // Leave room for 1 line below us + ImGui::Text("MyObject: %d", selected); + ImGui::Separator(); + if (ImGui::BeginTabBar("##Tabs", ImGuiTabBarFlags_None)) + { + if (ImGui::BeginTabItem("Description")) + { + ImGui::TextWrapped("Lorem ipsum dolor sit amet, consectetur adipiscing elit, sed do eiusmod tempor incididunt ut labore et dolore magna aliqua. "); + ImGui::EndTabItem(); + } + if (ImGui::BeginTabItem("Details")) + { + ImGui::Text("ID: 0123456789"); + ImGui::EndTabItem(); + } + ImGui::EndTabBar(); + } + ImGui::EndChild(); + if (ImGui::Button("Revert")) {} + ImGui::SameLine(); + if (ImGui::Button("Save")) {} + ImGui::EndGroup(); + } + } + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Property Editor / ShowExampleAppPropertyEditor() +//----------------------------------------------------------------------------- + +static void ShowPlaceholderObject(const char* prefix, int uid) +{ + // Use object uid as identifier. Most commonly you could also use the object pointer as a base ID. + ImGui::PushID(uid); + + // Text and Tree nodes are less high than framed widgets, using AlignTextToFramePadding() we add vertical spacing to make the tree lines equal high. + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::AlignTextToFramePadding(); + bool node_open = ImGui::TreeNode("Object", "%s_%u", prefix, uid); + ImGui::TableSetColumnIndex(1); + ImGui::Text("my sailor is rich"); + + if (node_open) + { + static float placeholder_members[8] = { 0.0f, 0.0f, 1.0f, 3.1416f, 100.0f, 999.0f }; + for (int i = 0; i < 8; i++) + { + ImGui::PushID(i); // Use field index as identifier. + if (i < 2) + { + ShowPlaceholderObject("Child", 424242); + } + else + { + // Here we use a TreeNode to highlight on hover (we could use e.g. Selectable as well) + ImGui::TableNextRow(); + ImGui::TableSetColumnIndex(0); + ImGui::AlignTextToFramePadding(); + ImGuiTreeNodeFlags flags = ImGuiTreeNodeFlags_Leaf | ImGuiTreeNodeFlags_NoTreePushOnOpen | ImGuiTreeNodeFlags_Bullet; + ImGui::TreeNodeEx("Field", flags, "Field_%d", i); + + ImGui::TableSetColumnIndex(1); + ImGui::SetNextItemWidth(-FLT_MIN); + if (i >= 5) + ImGui::InputFloat("##value", &placeholder_members[i], 1.0f); + else + ImGui::DragFloat("##value", &placeholder_members[i], 0.01f); + ImGui::NextColumn(); + } + ImGui::PopID(); + } + ImGui::TreePop(); + } + ImGui::PopID(); +} + +// Demonstrate create a simple property editor. +// This demo is a bit lackluster nowadays, would be nice to improve. +static void ShowExampleAppPropertyEditor(bool* p_open) +{ + ImGui::SetNextWindowSize(ImVec2(430, 450), ImGuiCond_FirstUseEver); + if (!ImGui::Begin("Example: Property editor", p_open)) + { + ImGui::End(); + return; + } + + IMGUI_DEMO_MARKER("Examples/Property Editor"); + HelpMarker( + "This example shows how you may implement a property editor using two columns.\n" + "All objects/fields data are dummies here.\n"); + + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, ImVec2(2, 2)); + if (ImGui::BeginTable("##split", 2, ImGuiTableFlags_BordersOuter | ImGuiTableFlags_Resizable | ImGuiTableFlags_ScrollY)) + { + ImGui::TableSetupScrollFreeze(0, 1); + ImGui::TableSetupColumn("Object"); + ImGui::TableSetupColumn("Contents"); + ImGui::TableHeadersRow(); + + // Iterate placeholder objects (all the same data) + for (int obj_i = 0; obj_i < 4; obj_i++) + ShowPlaceholderObject("Object", obj_i); + + ImGui::EndTable(); + } + ImGui::PopStyleVar(); + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Long Text / ShowExampleAppLongText() +//----------------------------------------------------------------------------- + +// Demonstrate/test rendering huge amount of text, and the incidence of clipping. +static void ShowExampleAppLongText(bool* p_open) +{ + ImGui::SetNextWindowSize(ImVec2(520, 600), ImGuiCond_FirstUseEver); + if (!ImGui::Begin("Example: Long text display", p_open)) + { + ImGui::End(); + return; + } + IMGUI_DEMO_MARKER("Examples/Long text display"); + + static int test_type = 0; + static ImGuiTextBuffer log; + static int lines = 0; + ImGui::Text("Printing unusually long amount of text."); + ImGui::Combo("Test type", &test_type, + "Single call to TextUnformatted()\0" + "Multiple calls to Text(), clipped\0" + "Multiple calls to Text(), not clipped (slow)\0"); + ImGui::Text("Buffer contents: %d lines, %d bytes", lines, log.size()); + if (ImGui::Button("Clear")) { log.clear(); lines = 0; } + ImGui::SameLine(); + if (ImGui::Button("Add 1000 lines")) + { + for (int i = 0; i < 1000; i++) + log.appendf("%i The quick brown fox jumps over the lazy dog\n", lines + i); + lines += 1000; + } + ImGui::BeginChild("Log"); + switch (test_type) + { + case 0: + // Single call to TextUnformatted() with a big buffer + ImGui::TextUnformatted(log.begin(), log.end()); + break; + case 1: + { + // Multiple calls to Text(), manually coarsely clipped - demonstrate how to use the ImGuiListClipper helper. + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(0, 0)); + ImGuiListClipper clipper; + clipper.Begin(lines); + while (clipper.Step()) + for (int i = clipper.DisplayStart; i < clipper.DisplayEnd; i++) + ImGui::Text("%i The quick brown fox jumps over the lazy dog", i); + ImGui::PopStyleVar(); + break; + } + case 2: + // Multiple calls to Text(), not clipped (slow) + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(0, 0)); + for (int i = 0; i < lines; i++) + ImGui::Text("%i The quick brown fox jumps over the lazy dog", i); + ImGui::PopStyleVar(); + break; + } + ImGui::EndChild(); + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Auto Resize / ShowExampleAppAutoResize() +//----------------------------------------------------------------------------- + +// Demonstrate creating a window which gets auto-resized according to its content. +static void ShowExampleAppAutoResize(bool* p_open) +{ + if (!ImGui::Begin("Example: Auto-resizing window", p_open, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::End(); + return; + } + IMGUI_DEMO_MARKER("Examples/Auto-resizing window"); + + static int lines = 10; + ImGui::TextUnformatted( + "Window will resize every-frame to the size of its content.\n" + "Note that you probably don't want to query the window size to\n" + "output your content because that would create a feedback loop."); + ImGui::SliderInt("Number of lines", &lines, 1, 20); + for (int i = 0; i < lines; i++) + ImGui::Text("%*sThis is line %d", i * 4, "", i); // Pad with space to extend size horizontally + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Constrained Resize / ShowExampleAppConstrainedResize() +//----------------------------------------------------------------------------- + +// Demonstrate creating a window with custom resize constraints. +// Note that size constraints currently don't work on a docked window (when in 'docking' branch) +static void ShowExampleAppConstrainedResize(bool* p_open) +{ + struct CustomConstraints + { + // Helper functions to demonstrate programmatic constraints + // FIXME: This doesn't take account of decoration size (e.g. title bar), library should make this easier. + static void AspectRatio(ImGuiSizeCallbackData* data) { float aspect_ratio = *(float*)data->UserData; data->DesiredSize.x = IM_MAX(data->CurrentSize.x, data->CurrentSize.y); data->DesiredSize.y = (float)(int)(data->DesiredSize.x / aspect_ratio); } + static void Square(ImGuiSizeCallbackData* data) { data->DesiredSize.x = data->DesiredSize.y = IM_MAX(data->CurrentSize.x, data->CurrentSize.y); } + static void Step(ImGuiSizeCallbackData* data) { float step = *(float*)data->UserData; data->DesiredSize = ImVec2((int)(data->CurrentSize.x / step + 0.5f) * step, (int)(data->CurrentSize.y / step + 0.5f) * step); } + }; + + const char* test_desc[] = + { + "Between 100x100 and 500x500", + "At least 100x100", + "Resize vertical only", + "Resize horizontal only", + "Width Between 400 and 500", + "Custom: Aspect Ratio 16:9", + "Custom: Always Square", + "Custom: Fixed Steps (100)", + }; + + // Options + static bool auto_resize = false; + static bool window_padding = true; + static int type = 5; // Aspect Ratio + static int display_lines = 10; + + // Submit constraint + float aspect_ratio = 16.0f / 9.0f; + float fixed_step = 100.0f; + if (type == 0) ImGui::SetNextWindowSizeConstraints(ImVec2(100, 100), ImVec2(500, 500)); // Between 100x100 and 500x500 + if (type == 1) ImGui::SetNextWindowSizeConstraints(ImVec2(100, 100), ImVec2(FLT_MAX, FLT_MAX)); // Width > 100, Height > 100 + if (type == 2) ImGui::SetNextWindowSizeConstraints(ImVec2(-1, 0), ImVec2(-1, FLT_MAX)); // Vertical only + if (type == 3) ImGui::SetNextWindowSizeConstraints(ImVec2(0, -1), ImVec2(FLT_MAX, -1)); // Horizontal only + if (type == 4) ImGui::SetNextWindowSizeConstraints(ImVec2(400, -1), ImVec2(500, -1)); // Width Between and 400 and 500 + if (type == 5) ImGui::SetNextWindowSizeConstraints(ImVec2(0, 0), ImVec2(FLT_MAX, FLT_MAX), CustomConstraints::AspectRatio, (void*)&aspect_ratio); // Aspect ratio + if (type == 6) ImGui::SetNextWindowSizeConstraints(ImVec2(0, 0), ImVec2(FLT_MAX, FLT_MAX), CustomConstraints::Square); // Always Square + if (type == 7) ImGui::SetNextWindowSizeConstraints(ImVec2(0, 0), ImVec2(FLT_MAX, FLT_MAX), CustomConstraints::Step, (void*)&fixed_step); // Fixed Step + + // Submit window + if (!window_padding) + ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0.0f, 0.0f)); + const ImGuiWindowFlags window_flags = auto_resize ? ImGuiWindowFlags_AlwaysAutoResize : 0; + const bool window_open = ImGui::Begin("Example: Constrained Resize", p_open, window_flags); + if (!window_padding) + ImGui::PopStyleVar(); + if (window_open) + { + IMGUI_DEMO_MARKER("Examples/Constrained Resizing window"); + if (ImGui::GetIO().KeyShift) + { + // Display a dummy viewport (in your real app you would likely use ImageButton() to display a texture. + ImVec2 avail_size = ImGui::GetContentRegionAvail(); + ImVec2 pos = ImGui::GetCursorScreenPos(); + ImGui::ColorButton("viewport", ImVec4(0.5f, 0.2f, 0.5f, 1.0f), ImGuiColorEditFlags_NoTooltip | ImGuiColorEditFlags_NoDragDrop, avail_size); + ImGui::SetCursorScreenPos(ImVec2(pos.x + 10, pos.y + 10)); + ImGui::Text("%.2f x %.2f", avail_size.x, avail_size.y); + } + else + { + ImGui::Text("(Hold SHIFT to display a dummy viewport)"); + if (ImGui::IsWindowDocked()) + ImGui::Text("Warning: Sizing Constraints won't work if the window is docked!"); + if (ImGui::Button("Set 200x200")) { ImGui::SetWindowSize(ImVec2(200, 200)); } ImGui::SameLine(); + if (ImGui::Button("Set 500x500")) { ImGui::SetWindowSize(ImVec2(500, 500)); } ImGui::SameLine(); + if (ImGui::Button("Set 800x200")) { ImGui::SetWindowSize(ImVec2(800, 200)); } + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 20); + ImGui::Combo("Constraint", &type, test_desc, IM_ARRAYSIZE(test_desc)); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 20); + ImGui::DragInt("Lines", &display_lines, 0.2f, 1, 100); + ImGui::Checkbox("Auto-resize", &auto_resize); + ImGui::Checkbox("Window padding", &window_padding); + for (int i = 0; i < display_lines; i++) + ImGui::Text("%*sHello, sailor! Making this line long enough for the example.", i * 4, ""); + } + } + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Simple overlay / ShowExampleAppSimpleOverlay() +//----------------------------------------------------------------------------- + +// Demonstrate creating a simple static window with no decoration +// + a context-menu to choose which corner of the screen to use. +static void ShowExampleAppSimpleOverlay(bool* p_open) +{ + static int location = 0; + ImGuiIO& io = ImGui::GetIO(); + ImGuiWindowFlags window_flags = ImGuiWindowFlags_NoDecoration | ImGuiWindowFlags_NoDocking | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoSavedSettings | ImGuiWindowFlags_NoFocusOnAppearing | ImGuiWindowFlags_NoNav; + if (location >= 0) + { + const float PAD = 10.0f; + const ImGuiViewport* viewport = ImGui::GetMainViewport(); + ImVec2 work_pos = viewport->WorkPos; // Use work area to avoid menu-bar/task-bar, if any! + ImVec2 work_size = viewport->WorkSize; + ImVec2 window_pos, window_pos_pivot; + window_pos.x = (location & 1) ? (work_pos.x + work_size.x - PAD) : (work_pos.x + PAD); + window_pos.y = (location & 2) ? (work_pos.y + work_size.y - PAD) : (work_pos.y + PAD); + window_pos_pivot.x = (location & 1) ? 1.0f : 0.0f; + window_pos_pivot.y = (location & 2) ? 1.0f : 0.0f; + ImGui::SetNextWindowPos(window_pos, ImGuiCond_Always, window_pos_pivot); + ImGui::SetNextWindowViewport(viewport->ID); + window_flags |= ImGuiWindowFlags_NoMove; + } + else if (location == -2) + { + // Center window + ImGui::SetNextWindowPos(ImGui::GetMainViewport()->GetCenter(), ImGuiCond_Always, ImVec2(0.5f, 0.5f)); + window_flags |= ImGuiWindowFlags_NoMove; + } + ImGui::SetNextWindowBgAlpha(0.35f); // Transparent background + if (ImGui::Begin("Example: Simple overlay", p_open, window_flags)) + { + IMGUI_DEMO_MARKER("Examples/Simple Overlay"); + ImGui::Text("Simple overlay\n" "(right-click to change position)"); + ImGui::Separator(); + if (ImGui::IsMousePosValid()) + ImGui::Text("Mouse Position: (%.1f,%.1f)", io.MousePos.x, io.MousePos.y); + else + ImGui::Text("Mouse Position: "); + if (ImGui::BeginPopupContextWindow()) + { + if (ImGui::MenuItem("Custom", NULL, location == -1)) location = -1; + if (ImGui::MenuItem("Center", NULL, location == -2)) location = -2; + if (ImGui::MenuItem("Top-left", NULL, location == 0)) location = 0; + if (ImGui::MenuItem("Top-right", NULL, location == 1)) location = 1; + if (ImGui::MenuItem("Bottom-left", NULL, location == 2)) location = 2; + if (ImGui::MenuItem("Bottom-right", NULL, location == 3)) location = 3; + if (p_open && ImGui::MenuItem("Close")) *p_open = false; + ImGui::EndPopup(); + } + } + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Fullscreen window / ShowExampleAppFullscreen() +//----------------------------------------------------------------------------- + +// Demonstrate creating a window covering the entire screen/viewport +static void ShowExampleAppFullscreen(bool* p_open) +{ + static bool use_work_area = true; + static ImGuiWindowFlags flags = ImGuiWindowFlags_NoDecoration | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoSavedSettings; + + // We demonstrate using the full viewport area or the work area (without menu-bars, task-bars etc.) + // Based on your use case you may want one or the other. + const ImGuiViewport* viewport = ImGui::GetMainViewport(); + ImGui::SetNextWindowPos(use_work_area ? viewport->WorkPos : viewport->Pos); + ImGui::SetNextWindowSize(use_work_area ? viewport->WorkSize : viewport->Size); + + if (ImGui::Begin("Example: Fullscreen window", p_open, flags)) + { + ImGui::Checkbox("Use work area instead of main area", &use_work_area); + ImGui::SameLine(); + HelpMarker("Main Area = entire viewport,\nWork Area = entire viewport minus sections used by the main menu bars, task bars etc.\n\nEnable the main-menu bar in Examples menu to see the difference."); + + ImGui::CheckboxFlags("ImGuiWindowFlags_NoBackground", &flags, ImGuiWindowFlags_NoBackground); + ImGui::CheckboxFlags("ImGuiWindowFlags_NoDecoration", &flags, ImGuiWindowFlags_NoDecoration); + ImGui::Indent(); + ImGui::CheckboxFlags("ImGuiWindowFlags_NoTitleBar", &flags, ImGuiWindowFlags_NoTitleBar); + ImGui::CheckboxFlags("ImGuiWindowFlags_NoCollapse", &flags, ImGuiWindowFlags_NoCollapse); + ImGui::CheckboxFlags("ImGuiWindowFlags_NoScrollbar", &flags, ImGuiWindowFlags_NoScrollbar); + ImGui::Unindent(); + + if (p_open && ImGui::Button("Close this window")) + *p_open = false; + } + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Manipulating Window Titles / ShowExampleAppWindowTitles() +//----------------------------------------------------------------------------- + +// Demonstrate the use of "##" and "###" in identifiers to manipulate ID generation. +// This applies to all regular items as well. +// Read FAQ section "How can I have multiple widgets with the same label?" for details. +static void ShowExampleAppWindowTitles(bool*) +{ + const ImGuiViewport* viewport = ImGui::GetMainViewport(); + const ImVec2 base_pos = viewport->Pos; + + // By default, Windows are uniquely identified by their title. + // You can use the "##" and "###" markers to manipulate the display/ID. + + // Using "##" to display same title but have unique identifier. + ImGui::SetNextWindowPos(ImVec2(base_pos.x + 100, base_pos.y + 100), ImGuiCond_FirstUseEver); + ImGui::Begin("Same title as another window##1"); + IMGUI_DEMO_MARKER("Examples/Manipulating window titles"); + ImGui::Text("This is window 1.\nMy title is the same as window 2, but my identifier is unique."); + ImGui::End(); + + ImGui::SetNextWindowPos(ImVec2(base_pos.x + 100, base_pos.y + 200), ImGuiCond_FirstUseEver); + ImGui::Begin("Same title as another window##2"); + ImGui::Text("This is window 2.\nMy title is the same as window 1, but my identifier is unique."); + ImGui::End(); + + // Using "###" to display a changing title but keep a static identifier "AnimatedTitle" + char buf[128]; + sprintf(buf, "Animated title %c %d###AnimatedTitle", "|/-\\"[(int)(ImGui::GetTime() / 0.25f) & 3], ImGui::GetFrameCount()); + ImGui::SetNextWindowPos(ImVec2(base_pos.x + 100, base_pos.y + 300), ImGuiCond_FirstUseEver); + ImGui::Begin(buf); + ImGui::Text("This window has a changing title."); + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Custom Rendering using ImDrawList API / ShowExampleAppCustomRendering() +//----------------------------------------------------------------------------- + +// Demonstrate using the low-level ImDrawList to draw custom shapes. +static void ShowExampleAppCustomRendering(bool* p_open) +{ + if (!ImGui::Begin("Example: Custom rendering", p_open)) + { + ImGui::End(); + return; + } + IMGUI_DEMO_MARKER("Examples/Custom Rendering"); + + // Tip: If you do a lot of custom rendering, you probably want to use your own geometrical types and benefit of + // overloaded operators, etc. Define IM_VEC2_CLASS_EXTRA in imconfig.h to create implicit conversions between your + // types and ImVec2/ImVec4. Dear ImGui defines overloaded operators but they are internal to imgui.cpp and not + // exposed outside (to avoid messing with your types) In this example we are not using the maths operators! + + if (ImGui::BeginTabBar("##TabBar")) + { + if (ImGui::BeginTabItem("Primitives")) + { + ImGui::PushItemWidth(-ImGui::GetFontSize() * 15); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + + // Draw gradients + // (note that those are currently exacerbating our sRGB/Linear issues) + // Calling ImGui::GetColorU32() multiplies the given colors by the current Style Alpha, but you may pass the IM_COL32() directly as well.. + ImGui::Text("Gradients"); + ImVec2 gradient_size = ImVec2(ImGui::CalcItemWidth(), ImGui::GetFrameHeight()); + { + ImVec2 p0 = ImGui::GetCursorScreenPos(); + ImVec2 p1 = ImVec2(p0.x + gradient_size.x, p0.y + gradient_size.y); + ImU32 col_a = ImGui::GetColorU32(IM_COL32(0, 0, 0, 255)); + ImU32 col_b = ImGui::GetColorU32(IM_COL32(255, 255, 255, 255)); + draw_list->AddRectFilledMultiColor(p0, p1, col_a, col_b, col_b, col_a); + ImGui::InvisibleButton("##gradient1", gradient_size); + } + { + ImVec2 p0 = ImGui::GetCursorScreenPos(); + ImVec2 p1 = ImVec2(p0.x + gradient_size.x, p0.y + gradient_size.y); + ImU32 col_a = ImGui::GetColorU32(IM_COL32(0, 255, 0, 255)); + ImU32 col_b = ImGui::GetColorU32(IM_COL32(255, 0, 0, 255)); + draw_list->AddRectFilledMultiColor(p0, p1, col_a, col_b, col_b, col_a); + ImGui::InvisibleButton("##gradient2", gradient_size); + } + + // Draw a bunch of primitives + ImGui::Text("All primitives"); + static float sz = 36.0f; + static float thickness = 3.0f; + static int ngon_sides = 6; + static bool circle_segments_override = false; + static int circle_segments_override_v = 12; + static bool curve_segments_override = false; + static int curve_segments_override_v = 8; + static ImVec4 colf = ImVec4(1.0f, 1.0f, 0.4f, 1.0f); + ImGui::DragFloat("Size", &sz, 0.2f, 2.0f, 100.0f, "%.0f"); + ImGui::DragFloat("Thickness", &thickness, 0.05f, 1.0f, 8.0f, "%.02f"); + ImGui::SliderInt("N-gon sides", &ngon_sides, 3, 12); + ImGui::Checkbox("##circlesegmentoverride", &circle_segments_override); + ImGui::SameLine(0.0f, ImGui::GetStyle().ItemInnerSpacing.x); + circle_segments_override |= ImGui::SliderInt("Circle segments override", &circle_segments_override_v, 3, 40); + ImGui::Checkbox("##curvessegmentoverride", &curve_segments_override); + ImGui::SameLine(0.0f, ImGui::GetStyle().ItemInnerSpacing.x); + curve_segments_override |= ImGui::SliderInt("Curves segments override", &curve_segments_override_v, 3, 40); + ImGui::ColorEdit4("Color", &colf.x); + + const ImVec2 p = ImGui::GetCursorScreenPos(); + const ImU32 col = ImColor(colf); + const float spacing = 10.0f; + const ImDrawFlags corners_tl_br = ImDrawFlags_RoundCornersTopLeft | ImDrawFlags_RoundCornersBottomRight; + const float rounding = sz / 5.0f; + const int circle_segments = circle_segments_override ? circle_segments_override_v : 0; + const int curve_segments = curve_segments_override ? curve_segments_override_v : 0; + float x = p.x + 4.0f; + float y = p.y + 4.0f; + for (int n = 0; n < 2; n++) + { + // First line uses a thickness of 1.0f, second line uses the configurable thickness + float th = (n == 0) ? 1.0f : thickness; + draw_list->AddNgon(ImVec2(x + sz*0.5f, y + sz*0.5f), sz*0.5f, col, ngon_sides, th); x += sz + spacing; // N-gon + draw_list->AddCircle(ImVec2(x + sz*0.5f, y + sz*0.5f), sz*0.5f, col, circle_segments, th); x += sz + spacing; // Circle + draw_list->AddEllipse(ImVec2(x + sz*0.5f, y + sz*0.5f), sz*0.5f, sz*0.3f, col, -0.3f, circle_segments, th); x += sz + spacing; // Ellipse + draw_list->AddRect(ImVec2(x, y), ImVec2(x + sz, y + sz), col, 0.0f, ImDrawFlags_None, th); x += sz + spacing; // Square + draw_list->AddRect(ImVec2(x, y), ImVec2(x + sz, y + sz), col, rounding, ImDrawFlags_None, th); x += sz + spacing; // Square with all rounded corners + draw_list->AddRect(ImVec2(x, y), ImVec2(x + sz, y + sz), col, rounding, corners_tl_br, th); x += sz + spacing; // Square with two rounded corners + draw_list->AddTriangle(ImVec2(x+sz*0.5f,y), ImVec2(x+sz, y+sz-0.5f), ImVec2(x, y+sz-0.5f), col, th);x += sz + spacing; // Triangle + //draw_list->AddTriangle(ImVec2(x+sz*0.2f,y), ImVec2(x, y+sz-0.5f), ImVec2(x+sz*0.4f, y+sz-0.5f), col, th);x+= sz*0.4f + spacing; // Thin triangle + draw_list->AddLine(ImVec2(x, y), ImVec2(x + sz, y), col, th); x += sz + spacing; // Horizontal line (note: drawing a filled rectangle will be faster!) + draw_list->AddLine(ImVec2(x, y), ImVec2(x, y + sz), col, th); x += spacing; // Vertical line (note: drawing a filled rectangle will be faster!) + draw_list->AddLine(ImVec2(x, y), ImVec2(x + sz, y + sz), col, th); x += sz + spacing; // Diagonal line + + // Quadratic Bezier Curve (3 control points) + ImVec2 cp3[3] = { ImVec2(x, y + sz * 0.6f), ImVec2(x + sz * 0.5f, y - sz * 0.4f), ImVec2(x + sz, y + sz) }; + draw_list->AddBezierQuadratic(cp3[0], cp3[1], cp3[2], col, th, curve_segments); x += sz + spacing; + + // Cubic Bezier Curve (4 control points) + ImVec2 cp4[4] = { ImVec2(x, y), ImVec2(x + sz * 1.3f, y + sz * 0.3f), ImVec2(x + sz - sz * 1.3f, y + sz - sz * 0.3f), ImVec2(x + sz, y + sz) }; + draw_list->AddBezierCubic(cp4[0], cp4[1], cp4[2], cp4[3], col, th, curve_segments); + + x = p.x + 4; + y += sz + spacing; + } + draw_list->AddNgonFilled(ImVec2(x + sz * 0.5f, y + sz * 0.5f), sz * 0.5f, col, ngon_sides); x += sz + spacing; // N-gon + draw_list->AddCircleFilled(ImVec2(x + sz * 0.5f, y + sz * 0.5f), sz * 0.5f, col, circle_segments); x += sz + spacing; // Circle + draw_list->AddEllipseFilled(ImVec2(x + sz * 0.5f, y + sz * 0.5f), sz * 0.5f, sz * 0.3f, col, -0.3f, circle_segments); x += sz + spacing;// Ellipse + draw_list->AddRectFilled(ImVec2(x, y), ImVec2(x + sz, y + sz), col); x += sz + spacing; // Square + draw_list->AddRectFilled(ImVec2(x, y), ImVec2(x + sz, y + sz), col, 10.0f); x += sz + spacing; // Square with all rounded corners + draw_list->AddRectFilled(ImVec2(x, y), ImVec2(x + sz, y + sz), col, 10.0f, corners_tl_br); x += sz + spacing; // Square with two rounded corners + draw_list->AddTriangleFilled(ImVec2(x+sz*0.5f,y), ImVec2(x+sz, y+sz-0.5f), ImVec2(x, y+sz-0.5f), col); x += sz + spacing; // Triangle + //draw_list->AddTriangleFilled(ImVec2(x+sz*0.2f,y), ImVec2(x, y+sz-0.5f), ImVec2(x+sz*0.4f, y+sz-0.5f), col); x += sz*0.4f + spacing; // Thin triangle + draw_list->AddRectFilled(ImVec2(x, y), ImVec2(x + sz, y + thickness), col); x += sz + spacing; // Horizontal line (faster than AddLine, but only handle integer thickness) + draw_list->AddRectFilled(ImVec2(x, y), ImVec2(x + thickness, y + sz), col); x += spacing * 2.0f;// Vertical line (faster than AddLine, but only handle integer thickness) + draw_list->AddRectFilled(ImVec2(x, y), ImVec2(x + 1, y + 1), col); x += sz; // Pixel (faster than AddLine) + draw_list->AddRectFilledMultiColor(ImVec2(x, y), ImVec2(x + sz, y + sz), IM_COL32(0, 0, 0, 255), IM_COL32(255, 0, 0, 255), IM_COL32(255, 255, 0, 255), IM_COL32(0, 255, 0, 255)); + + ImGui::Dummy(ImVec2((sz + spacing) * 11.2f, (sz + spacing) * 3.0f)); + ImGui::PopItemWidth(); + ImGui::EndTabItem(); + } + + if (ImGui::BeginTabItem("Canvas")) + { + static ImVector points; + static ImVec2 scrolling(0.0f, 0.0f); + static bool opt_enable_grid = true; + static bool opt_enable_context_menu = true; + static bool adding_line = false; + + ImGui::Checkbox("Enable grid", &opt_enable_grid); + ImGui::Checkbox("Enable context menu", &opt_enable_context_menu); + ImGui::Text("Mouse Left: drag to add lines,\nMouse Right: drag to scroll, click for context menu."); + + // Typically you would use a BeginChild()/EndChild() pair to benefit from a clipping region + own scrolling. + // Here we demonstrate that this can be replaced by simple offsetting + custom drawing + PushClipRect/PopClipRect() calls. + // To use a child window instead we could use, e.g: + // ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0, 0)); // Disable padding + // ImGui::PushStyleColor(ImGuiCol_ChildBg, IM_COL32(50, 50, 50, 255)); // Set a background color + // ImGui::BeginChild("canvas", ImVec2(0.0f, 0.0f), true, ImGuiWindowFlags_NoMove); + // ImGui::PopStyleColor(); + // ImGui::PopStyleVar(); + // [...] + // ImGui::EndChild(); + + // Using InvisibleButton() as a convenience 1) it will advance the layout cursor and 2) allows us to use IsItemHovered()/IsItemActive() + ImVec2 canvas_p0 = ImGui::GetCursorScreenPos(); // ImDrawList API uses screen coordinates! + ImVec2 canvas_sz = ImGui::GetContentRegionAvail(); // Resize canvas to what's available + if (canvas_sz.x < 50.0f) canvas_sz.x = 50.0f; + if (canvas_sz.y < 50.0f) canvas_sz.y = 50.0f; + ImVec2 canvas_p1 = ImVec2(canvas_p0.x + canvas_sz.x, canvas_p0.y + canvas_sz.y); + + // Draw border and background color + ImGuiIO& io = ImGui::GetIO(); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + draw_list->AddRectFilled(canvas_p0, canvas_p1, IM_COL32(50, 50, 50, 255)); + draw_list->AddRect(canvas_p0, canvas_p1, IM_COL32(255, 255, 255, 255)); + + // This will catch our interactions + ImGui::InvisibleButton("canvas", canvas_sz, ImGuiButtonFlags_MouseButtonLeft | ImGuiButtonFlags_MouseButtonRight); + const bool is_hovered = ImGui::IsItemHovered(); // Hovered + const bool is_active = ImGui::IsItemActive(); // Held + const ImVec2 origin(canvas_p0.x + scrolling.x, canvas_p0.y + scrolling.y); // Lock scrolled origin + const ImVec2 mouse_pos_in_canvas(io.MousePos.x - origin.x, io.MousePos.y - origin.y); + + // Add first and second point + if (is_hovered && !adding_line && ImGui::IsMouseClicked(ImGuiMouseButton_Left)) + { + points.push_back(mouse_pos_in_canvas); + points.push_back(mouse_pos_in_canvas); + adding_line = true; + } + if (adding_line) + { + points.back() = mouse_pos_in_canvas; + if (!ImGui::IsMouseDown(ImGuiMouseButton_Left)) + adding_line = false; + } + + // Pan (we use a zero mouse threshold when there's no context menu) + // You may decide to make that threshold dynamic based on whether the mouse is hovering something etc. + const float mouse_threshold_for_pan = opt_enable_context_menu ? -1.0f : 0.0f; + if (is_active && ImGui::IsMouseDragging(ImGuiMouseButton_Right, mouse_threshold_for_pan)) + { + scrolling.x += io.MouseDelta.x; + scrolling.y += io.MouseDelta.y; + } + + // Context menu (under default mouse threshold) + ImVec2 drag_delta = ImGui::GetMouseDragDelta(ImGuiMouseButton_Right); + if (opt_enable_context_menu && drag_delta.x == 0.0f && drag_delta.y == 0.0f) + ImGui::OpenPopupOnItemClick("context", ImGuiPopupFlags_MouseButtonRight); + if (ImGui::BeginPopup("context")) + { + if (adding_line) + points.resize(points.size() - 2); + adding_line = false; + if (ImGui::MenuItem("Remove one", NULL, false, points.Size > 0)) { points.resize(points.size() - 2); } + if (ImGui::MenuItem("Remove all", NULL, false, points.Size > 0)) { points.clear(); } + ImGui::EndPopup(); + } + + // Draw grid + all lines in the canvas + draw_list->PushClipRect(canvas_p0, canvas_p1, true); + if (opt_enable_grid) + { + const float GRID_STEP = 64.0f; + for (float x = fmodf(scrolling.x, GRID_STEP); x < canvas_sz.x; x += GRID_STEP) + draw_list->AddLine(ImVec2(canvas_p0.x + x, canvas_p0.y), ImVec2(canvas_p0.x + x, canvas_p1.y), IM_COL32(200, 200, 200, 40)); + for (float y = fmodf(scrolling.y, GRID_STEP); y < canvas_sz.y; y += GRID_STEP) + draw_list->AddLine(ImVec2(canvas_p0.x, canvas_p0.y + y), ImVec2(canvas_p1.x, canvas_p0.y + y), IM_COL32(200, 200, 200, 40)); + } + for (int n = 0; n < points.Size; n += 2) + draw_list->AddLine(ImVec2(origin.x + points[n].x, origin.y + points[n].y), ImVec2(origin.x + points[n + 1].x, origin.y + points[n + 1].y), IM_COL32(255, 255, 0, 255), 2.0f); + draw_list->PopClipRect(); + + ImGui::EndTabItem(); + } + + if (ImGui::BeginTabItem("BG/FG draw lists")) + { + static bool draw_bg = true; + static bool draw_fg = true; + ImGui::Checkbox("Draw in Background draw list", &draw_bg); + ImGui::SameLine(); HelpMarker("The Background draw list will be rendered below every Dear ImGui windows."); + ImGui::Checkbox("Draw in Foreground draw list", &draw_fg); + ImGui::SameLine(); HelpMarker("The Foreground draw list will be rendered over every Dear ImGui windows."); + ImVec2 window_pos = ImGui::GetWindowPos(); + ImVec2 window_size = ImGui::GetWindowSize(); + ImVec2 window_center = ImVec2(window_pos.x + window_size.x * 0.5f, window_pos.y + window_size.y * 0.5f); + if (draw_bg) + ImGui::GetBackgroundDrawList()->AddCircle(window_center, window_size.x * 0.6f, IM_COL32(255, 0, 0, 200), 0, 10 + 4); + if (draw_fg) + ImGui::GetForegroundDrawList()->AddCircle(window_center, window_size.y * 0.6f, IM_COL32(0, 255, 0, 200), 0, 10); + ImGui::EndTabItem(); + } + + // Demonstrate out-of-order rendering via channels splitting + // We use functions in ImDrawList as each draw list contains a convenience splitter, + // but you can also instantiate your own ImDrawListSplitter if you need to nest them. + if (ImGui::BeginTabItem("Draw Channels")) + { + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + { + ImGui::Text("Blue shape is drawn first: appears in back"); + ImGui::Text("Red shape is drawn after: appears in front"); + ImVec2 p0 = ImGui::GetCursorScreenPos(); + draw_list->AddRectFilled(ImVec2(p0.x, p0.y), ImVec2(p0.x + 50, p0.y + 50), IM_COL32(0, 0, 255, 255)); // Blue + draw_list->AddRectFilled(ImVec2(p0.x + 25, p0.y + 25), ImVec2(p0.x + 75, p0.y + 75), IM_COL32(255, 0, 0, 255)); // Red + ImGui::Dummy(ImVec2(75, 75)); + } + ImGui::Separator(); + { + ImGui::Text("Blue shape is drawn first, into channel 1: appears in front"); + ImGui::Text("Red shape is drawn after, into channel 0: appears in back"); + ImVec2 p1 = ImGui::GetCursorScreenPos(); + + // Create 2 channels and draw a Blue shape THEN a Red shape. + // You can create any number of channels. Tables API use 1 channel per column in order to better batch draw calls. + draw_list->ChannelsSplit(2); + draw_list->ChannelsSetCurrent(1); + draw_list->AddRectFilled(ImVec2(p1.x, p1.y), ImVec2(p1.x + 50, p1.y + 50), IM_COL32(0, 0, 255, 255)); // Blue + draw_list->ChannelsSetCurrent(0); + draw_list->AddRectFilled(ImVec2(p1.x + 25, p1.y + 25), ImVec2(p1.x + 75, p1.y + 75), IM_COL32(255, 0, 0, 255)); // Red + + // Flatten/reorder channels. Red shape is in channel 0 and it appears below the Blue shape in channel 1. + // This works by copying draw indices only (vertices are not copied). + draw_list->ChannelsMerge(); + ImGui::Dummy(ImVec2(75, 75)); + ImGui::Text("After reordering, contents of channel 0 appears below channel 1."); + } + ImGui::EndTabItem(); + } + + ImGui::EndTabBar(); + } + + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Docking, DockSpace / ShowExampleAppDockSpace() +//----------------------------------------------------------------------------- + +// Demonstrate using DockSpace() to create an explicit docking node within an existing window. +// Note: You can use most Docking facilities without calling any API. You DO NOT need to call DockSpace() to use Docking! +// - Drag from window title bar or their tab to dock/undock. Hold SHIFT to disable docking. +// - Drag from window menu button (upper-left button) to undock an entire node (all windows). +// - When io.ConfigDockingWithShift == true, you instead need to hold SHIFT to enable docking. +// About dockspaces: +// - Use DockSpace() to create an explicit dock node _within_ an existing window. +// - Use DockSpaceOverViewport() to create an explicit dock node covering the screen or a specific viewport. +// This is often used with ImGuiDockNodeFlags_PassthruCentralNode. +// - Important: Dockspaces need to be submitted _before_ any window they can host. Submit it early in your frame! (*) +// - Important: Dockspaces need to be kept alive if hidden, otherwise windows docked into it will be undocked. +// e.g. if you have multiple tabs with a dockspace inside each tab: submit the non-visible dockspaces with ImGuiDockNodeFlags_KeepAliveOnly. +// (*) because of this constraint, the implicit \"Debug\" window can not be docked into an explicit DockSpace() node, +// because that window is submitted as part of the part of the NewFrame() call. An easy workaround is that you can create +// your own implicit "Debug##2" window after calling DockSpace() and leave it in the window stack for anyone to use. +void ShowExampleAppDockSpace(bool* p_open) +{ + // READ THIS !!! + // TL;DR; this demo is more complicated than what most users you would normally use. + // If we remove all options we are showcasing, this demo would become: + // void ShowExampleAppDockSpace() + // { + // ImGui::DockSpaceOverViewport(ImGui::GetMainViewport()); + // } + // In most cases you should be able to just call DockSpaceOverViewport() and ignore all the code below! + // In this specific demo, we are not using DockSpaceOverViewport() because: + // - (1) we allow the host window to be floating/moveable instead of filling the viewport (when opt_fullscreen == false) + // - (2) we allow the host window to have padding (when opt_padding == true) + // - (3) we expose many flags and need a way to have them visible. + // - (4) we have a local menu bar in the host window (vs. you could use BeginMainMenuBar() + DockSpaceOverViewport() + // in your code, but we don't here because we allow the window to be floating) + + static bool opt_fullscreen = true; + static bool opt_padding = false; + static ImGuiDockNodeFlags dockspace_flags = ImGuiDockNodeFlags_None; + + // We are using the ImGuiWindowFlags_NoDocking flag to make the parent window not dockable into, + // because it would be confusing to have two docking targets within each others. + ImGuiWindowFlags window_flags = ImGuiWindowFlags_MenuBar | ImGuiWindowFlags_NoDocking; + if (opt_fullscreen) + { + const ImGuiViewport* viewport = ImGui::GetMainViewport(); + ImGui::SetNextWindowPos(viewport->WorkPos); + ImGui::SetNextWindowSize(viewport->WorkSize); + ImGui::SetNextWindowViewport(viewport->ID); + ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f); + ImGui::PushStyleVar(ImGuiStyleVar_WindowBorderSize, 0.0f); + window_flags |= ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove; + window_flags |= ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoNavFocus; + } + else + { + dockspace_flags &= ~ImGuiDockNodeFlags_PassthruCentralNode; + } + + // When using ImGuiDockNodeFlags_PassthruCentralNode, DockSpace() will render our background + // and handle the pass-thru hole, so we ask Begin() to not render a background. + if (dockspace_flags & ImGuiDockNodeFlags_PassthruCentralNode) + window_flags |= ImGuiWindowFlags_NoBackground; + + // Important: note that we proceed even if Begin() returns false (aka window is collapsed). + // This is because we want to keep our DockSpace() active. If a DockSpace() is inactive, + // all active windows docked into it will lose their parent and become undocked. + // We cannot preserve the docking relationship between an active window and an inactive docking, otherwise + // any change of dockspace/settings would lead to windows being stuck in limbo and never being visible. + if (!opt_padding) + ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, ImVec2(0.0f, 0.0f)); + ImGui::Begin("DockSpace Demo", p_open, window_flags); + if (!opt_padding) + ImGui::PopStyleVar(); + + if (opt_fullscreen) + ImGui::PopStyleVar(2); + + // Submit the DockSpace + ImGuiIO& io = ImGui::GetIO(); + if (io.ConfigFlags & ImGuiConfigFlags_DockingEnable) + { + ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); + ImGui::DockSpace(dockspace_id, ImVec2(0.0f, 0.0f), dockspace_flags); + } + else + { + ShowDockingDisabledMessage(); + } + + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("Options")) + { + // Disabling fullscreen would allow the window to be moved to the front of other windows, + // which we can't undo at the moment without finer window depth/z control. + ImGui::MenuItem("Fullscreen", NULL, &opt_fullscreen); + ImGui::MenuItem("Padding", NULL, &opt_padding); + ImGui::Separator(); + + if (ImGui::MenuItem("Flag: NoDockingOverCentralNode", "", (dockspace_flags & ImGuiDockNodeFlags_NoDockingOverCentralNode) != 0)) { dockspace_flags ^= ImGuiDockNodeFlags_NoDockingOverCentralNode; } + if (ImGui::MenuItem("Flag: NoDockingSplit", "", (dockspace_flags & ImGuiDockNodeFlags_NoDockingSplit) != 0)) { dockspace_flags ^= ImGuiDockNodeFlags_NoDockingSplit; } + if (ImGui::MenuItem("Flag: NoUndocking", "", (dockspace_flags & ImGuiDockNodeFlags_NoUndocking) != 0)) { dockspace_flags ^= ImGuiDockNodeFlags_NoUndocking; } + if (ImGui::MenuItem("Flag: NoResize", "", (dockspace_flags & ImGuiDockNodeFlags_NoResize) != 0)) { dockspace_flags ^= ImGuiDockNodeFlags_NoResize; } + if (ImGui::MenuItem("Flag: AutoHideTabBar", "", (dockspace_flags & ImGuiDockNodeFlags_AutoHideTabBar) != 0)) { dockspace_flags ^= ImGuiDockNodeFlags_AutoHideTabBar; } + if (ImGui::MenuItem("Flag: PassthruCentralNode", "", (dockspace_flags & ImGuiDockNodeFlags_PassthruCentralNode) != 0, opt_fullscreen)) { dockspace_flags ^= ImGuiDockNodeFlags_PassthruCentralNode; } + ImGui::Separator(); + + if (ImGui::MenuItem("Close", NULL, false, p_open != NULL)) + *p_open = false; + ImGui::EndMenu(); + } + HelpMarker( + "When docking is enabled, you can ALWAYS dock MOST window into another! Try it now!" "\n" + "- Drag from window title bar or their tab to dock/undock." "\n" + "- Drag from window menu button (upper-left button) to undock an entire node (all windows)." "\n" + "- Hold SHIFT to disable docking (if io.ConfigDockingWithShift == false, default)" "\n" + "- Hold SHIFT to enable docking (if io.ConfigDockingWithShift == true)" "\n" + "This demo app has nothing to do with enabling docking!" "\n\n" + "This demo app only demonstrate the use of ImGui::DockSpace() which allows you to manually create a docking node _within_ another window." "\n\n" + "Read comments in ShowExampleAppDockSpace() for more details."); + + ImGui::EndMenuBar(); + } + + ImGui::End(); +} + +//----------------------------------------------------------------------------- +// [SECTION] Example App: Documents Handling / ShowExampleAppDocuments() +//----------------------------------------------------------------------------- + +// Simplified structure to mimic a Document model +struct MyDocument +{ + const char* Name; // Document title + bool Open; // Set when open (we keep an array of all available documents to simplify demo code!) + bool OpenPrev; // Copy of Open from last update. + bool Dirty; // Set when the document has been modified + bool WantClose; // Set when the document + ImVec4 Color; // An arbitrary variable associated to the document + + MyDocument(const char* name, bool open = true, const ImVec4& color = ImVec4(1.0f, 1.0f, 1.0f, 1.0f)) + { + Name = name; + Open = OpenPrev = open; + Dirty = false; + WantClose = false; + Color = color; + } + void DoOpen() { Open = true; } + void DoQueueClose() { WantClose = true; } + void DoForceClose() { Open = false; Dirty = false; } + void DoSave() { Dirty = false; } + + // Display placeholder contents for the Document + static void DisplayContents(MyDocument* doc) + { + ImGui::PushID(doc); + ImGui::Text("Document \"%s\"", doc->Name); + ImGui::PushStyleColor(ImGuiCol_Text, doc->Color); + ImGui::TextWrapped("Lorem ipsum dolor sit amet, consectetur adipiscing elit, sed do eiusmod tempor incididunt ut labore et dolore magna aliqua."); + ImGui::PopStyleColor(); + if (ImGui::Button("Modify", ImVec2(100, 0))) + doc->Dirty = true; + ImGui::SameLine(); + if (ImGui::Button("Save", ImVec2(100, 0))) + doc->DoSave(); + ImGui::ColorEdit3("color", &doc->Color.x); // Useful to test drag and drop and hold-dragged-to-open-tab behavior. + ImGui::PopID(); + } + + // Display context menu for the Document + static void DisplayContextMenu(MyDocument* doc) + { + if (!ImGui::BeginPopupContextItem()) + return; + + char buf[256]; + sprintf(buf, "Save %s", doc->Name); + if (ImGui::MenuItem(buf, "CTRL+S", false, doc->Open)) + doc->DoSave(); + if (ImGui::MenuItem("Close", "CTRL+W", false, doc->Open)) + doc->DoQueueClose(); + ImGui::EndPopup(); + } +}; + +struct ExampleAppDocuments +{ + ImVector Documents; + + ExampleAppDocuments() + { + Documents.push_back(MyDocument("Lettuce", true, ImVec4(0.4f, 0.8f, 0.4f, 1.0f))); + Documents.push_back(MyDocument("Eggplant", true, ImVec4(0.8f, 0.5f, 1.0f, 1.0f))); + Documents.push_back(MyDocument("Carrot", true, ImVec4(1.0f, 0.8f, 0.5f, 1.0f))); + Documents.push_back(MyDocument("Tomato", false, ImVec4(1.0f, 0.3f, 0.4f, 1.0f))); + Documents.push_back(MyDocument("A Rather Long Title", false)); + Documents.push_back(MyDocument("Some Document", false)); + } +}; + +// [Optional] Notify the system of Tabs/Windows closure that happened outside the regular tab interface. +// If a tab has been closed programmatically (aka closed from another source such as the Checkbox() in the demo, +// as opposed to clicking on the regular tab closing button) and stops being submitted, it will take a frame for +// the tab bar to notice its absence. During this frame there will be a gap in the tab bar, and if the tab that has +// disappeared was the selected one, the tab bar will report no selected tab during the frame. This will effectively +// give the impression of a flicker for one frame. +// We call SetTabItemClosed() to manually notify the Tab Bar or Docking system of removed tabs to avoid this glitch. +// Note that this completely optional, and only affect tab bars with the ImGuiTabBarFlags_Reorderable flag. +static void NotifyOfDocumentsClosedElsewhere(ExampleAppDocuments& app) +{ + for (MyDocument& doc : app.Documents) + { + if (!doc.Open && doc.OpenPrev) + ImGui::SetTabItemClosed(doc.Name); + doc.OpenPrev = doc.Open; + } +} + +void ShowExampleAppDocuments(bool* p_open) +{ + static ExampleAppDocuments app; + + // Options + enum Target + { + Target_None, + Target_Tab, // Create documents as local tab into a local tab bar + Target_DockSpaceAndWindow // Create documents as regular windows, and create an embedded dockspace + }; + static Target opt_target = Target_Tab; + static bool opt_reorderable = true; + static ImGuiTabBarFlags opt_fitting_flags = ImGuiTabBarFlags_FittingPolicyDefault_; + + // When (opt_target == Target_DockSpaceAndWindow) there is the possibily that one of our child Document window (e.g. "Eggplant") + // that we emit gets docked into the same spot as the parent window ("Example: Documents"). + // This would create a problematic feedback loop because selecting the "Eggplant" tab would make the "Example: Documents" tab + // not visible, which in turn would stop submitting the "Eggplant" window. + // We avoid this problem by submitting our documents window even if our parent window is not currently visible. + // Another solution may be to make the "Example: Documents" window use the ImGuiWindowFlags_NoDocking. + + bool window_contents_visible = ImGui::Begin("Example: Documents", p_open, ImGuiWindowFlags_MenuBar); + if (!window_contents_visible && opt_target != Target_DockSpaceAndWindow) + { + ImGui::End(); + return; + } + + // Menu + if (ImGui::BeginMenuBar()) + { + if (ImGui::BeginMenu("File")) + { + int open_count = 0; + for (MyDocument& doc : app.Documents) + open_count += doc.Open ? 1 : 0; + + if (ImGui::BeginMenu("Open", open_count < app.Documents.Size)) + { + for (MyDocument& doc : app.Documents) + if (!doc.Open && ImGui::MenuItem(doc.Name)) + doc.DoOpen(); + ImGui::EndMenu(); + } + if (ImGui::MenuItem("Close All Documents", NULL, false, open_count > 0)) + for (MyDocument& doc : app.Documents) + doc.DoQueueClose(); + if (ImGui::MenuItem("Exit", "Ctrl+F4") && p_open) + *p_open = false; + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + } + + // [Debug] List documents with one checkbox for each + for (int doc_n = 0; doc_n < app.Documents.Size; doc_n++) + { + MyDocument& doc = app.Documents[doc_n]; + if (doc_n > 0) + ImGui::SameLine(); + ImGui::PushID(&doc); + if (ImGui::Checkbox(doc.Name, &doc.Open)) + if (!doc.Open) + doc.DoForceClose(); + ImGui::PopID(); + } + ImGui::PushItemWidth(ImGui::GetFontSize() * 12); + ImGui::Combo("Output", (int*)&opt_target, "None\0TabBar+Tabs\0DockSpace+Window\0"); + ImGui::PopItemWidth(); + bool redock_all = false; + if (opt_target == Target_Tab) { ImGui::SameLine(); ImGui::Checkbox("Reorderable Tabs", &opt_reorderable); } + if (opt_target == Target_DockSpaceAndWindow) { ImGui::SameLine(); redock_all = ImGui::Button("Redock all"); } + + ImGui::Separator(); + + // About the ImGuiWindowFlags_UnsavedDocument / ImGuiTabItemFlags_UnsavedDocument flags. + // They have multiple effects: + // - Display a dot next to the title. + // - Tab is selected when clicking the X close button. + // - Closure is not assumed (will wait for user to stop submitting the tab). + // Otherwise closure is assumed when pressing the X, so if you keep submitting the tab may reappear at end of tab bar. + // We need to assume closure by default otherwise waiting for "lack of submission" on the next frame would leave an empty + // hole for one-frame, both in the tab-bar and in tab-contents when closing a tab/window. + // The rarely used SetTabItemClosed() function is a way to notify of programmatic closure to avoid the one-frame hole. + + // Tabs + if (opt_target == Target_Tab) + { + ImGuiTabBarFlags tab_bar_flags = (opt_fitting_flags) | (opt_reorderable ? ImGuiTabBarFlags_Reorderable : 0); + if (ImGui::BeginTabBar("##tabs", tab_bar_flags)) + { + if (opt_reorderable) + NotifyOfDocumentsClosedElsewhere(app); + + // [DEBUG] Stress tests + //if ((ImGui::GetFrameCount() % 30) == 0) docs[1].Open ^= 1; // [DEBUG] Automatically show/hide a tab. Test various interactions e.g. dragging with this on. + //if (ImGui::GetIO().KeyCtrl) ImGui::SetTabItemSelected(docs[1].Name); // [DEBUG] Test SetTabItemSelected(), probably not very useful as-is anyway.. + + // Submit Tabs + for (MyDocument& doc : app.Documents) + { + if (!doc.Open) + continue; + + ImGuiTabItemFlags tab_flags = (doc.Dirty ? ImGuiTabItemFlags_UnsavedDocument : 0); + bool visible = ImGui::BeginTabItem(doc.Name, &doc.Open, tab_flags); + + // Cancel attempt to close when unsaved add to save queue so we can display a popup. + if (!doc.Open && doc.Dirty) + { + doc.Open = true; + doc.DoQueueClose(); + } + + MyDocument::DisplayContextMenu(&doc); + if (visible) + { + MyDocument::DisplayContents(&doc); + ImGui::EndTabItem(); + } + } + + ImGui::EndTabBar(); + } + } + else if (opt_target == Target_DockSpaceAndWindow) + { + if (ImGui::GetIO().ConfigFlags & ImGuiConfigFlags_DockingEnable) + { + NotifyOfDocumentsClosedElsewhere(app); + + // Create a DockSpace node where any window can be docked + ImGuiID dockspace_id = ImGui::GetID("MyDockSpace"); + ImGui::DockSpace(dockspace_id); + + // Create Windows + for (int doc_n = 0; doc_n < app.Documents.Size; doc_n++) + { + MyDocument* doc = &app.Documents[doc_n]; + if (!doc->Open) + continue; + + ImGui::SetNextWindowDockID(dockspace_id, redock_all ? ImGuiCond_Always : ImGuiCond_FirstUseEver); + ImGuiWindowFlags window_flags = (doc->Dirty ? ImGuiWindowFlags_UnsavedDocument : 0); + bool visible = ImGui::Begin(doc->Name, &doc->Open, window_flags); + + // Cancel attempt to close when unsaved add to save queue so we can display a popup. + if (!doc->Open && doc->Dirty) + { + doc->Open = true; + doc->DoQueueClose(); + } + + MyDocument::DisplayContextMenu(doc); + if (visible) + MyDocument::DisplayContents(doc); + + ImGui::End(); + } + } + else + { + ShowDockingDisabledMessage(); + } + } + + // Early out other contents + if (!window_contents_visible) + { + ImGui::End(); + return; + } + + // Update closing queue + static ImVector close_queue; + if (close_queue.empty()) + { + // Close queue is locked once we started a popup + for (MyDocument& doc : app.Documents) + if (doc.WantClose) + { + doc.WantClose = false; + close_queue.push_back(&doc); + } + } + + // Display closing confirmation UI + if (!close_queue.empty()) + { + int close_queue_unsaved_documents = 0; + for (int n = 0; n < close_queue.Size; n++) + if (close_queue[n]->Dirty) + close_queue_unsaved_documents++; + + if (close_queue_unsaved_documents == 0) + { + // Close documents when all are unsaved + for (int n = 0; n < close_queue.Size; n++) + close_queue[n]->DoForceClose(); + close_queue.clear(); + } + else + { + if (!ImGui::IsPopupOpen("Save?")) + ImGui::OpenPopup("Save?"); + if (ImGui::BeginPopupModal("Save?", NULL, ImGuiWindowFlags_AlwaysAutoResize)) + { + ImGui::Text("Save change to the following items?"); + float item_height = ImGui::GetTextLineHeightWithSpacing(); + if (ImGui::BeginChildFrame(ImGui::GetID("frame"), ImVec2(-FLT_MIN, 6.25f * item_height))) + { + for (int n = 0; n < close_queue.Size; n++) + if (close_queue[n]->Dirty) + ImGui::Text("%s", close_queue[n]->Name); + } + ImGui::EndChildFrame(); + + ImVec2 button_size(ImGui::GetFontSize() * 7.0f, 0.0f); + if (ImGui::Button("Yes", button_size)) + { + for (int n = 0; n < close_queue.Size; n++) + { + if (close_queue[n]->Dirty) + close_queue[n]->DoSave(); + close_queue[n]->DoForceClose(); + } + close_queue.clear(); + ImGui::CloseCurrentPopup(); + } + ImGui::SameLine(); + if (ImGui::Button("No", button_size)) + { + for (int n = 0; n < close_queue.Size; n++) + close_queue[n]->DoForceClose(); + close_queue.clear(); + ImGui::CloseCurrentPopup(); + } + ImGui::SameLine(); + if (ImGui::Button("Cancel", button_size)) + { + close_queue.clear(); + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + } + } + + ImGui::End(); +} + +// End of Demo code +#else + +void ImGui::ShowAboutWindow(bool*) {} +void ImGui::ShowDemoWindow(bool*) {} +void ImGui::ShowUserGuide() {} +void ImGui::ShowStyleEditor(ImGuiStyle*) {} + +#endif + +#endif // #ifndef IMGUI_DISABLE diff --git a/upstream_utils/imgui.py b/upstream_utils/imgui.py index 170cf41991..76dce5e94d 100755 --- a/upstream_utils/imgui.py +++ b/upstream_utils/imgui.py @@ -48,6 +48,7 @@ def copy_upstream_src(wpilib_root): "./backends/imgui_impl_opengl2.cpp", "./backends/imgui_impl_opengl3.cpp", "./imgui.cpp", + "./imgui_demo.cpp", "./imgui_draw.cpp", "./imgui_tables.cpp", "./imgui_widgets.cpp", diff --git a/upstream_utils/libdogleg.py b/upstream_utils/libdogleg.py new file mode 100755 index 0000000000..68ab38326a --- /dev/null +++ b/upstream_utils/libdogleg.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import Lib, walk_cwd_and_copy_if + + +def copy_upstream_src(wpilib_root): + wpical = os.path.join(wpilib_root, "wpical") + + # Delete old install + for d in [ + "src/main/native/thirdparty/libdogleg/src", + "src/main/native/thirdparty/libdogleg/include", + ]: + shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith("dogleg.h"), + os.path.join(wpical, "src/main/native/thirdparty/libdogleg/include"), + ) + for f in files: + with open(f) as file: + content = file.read() + content = content.replace( + "#include ", "#include " + ) + with open(f, "w") as file: + file.write(content) + + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith("dogleg.cpp"), + os.path.join(wpical, "src/main/native/thirdparty/libdogleg/src"), + ) + for f in files: + with open(f) as file: + content = file.read() + content = content.replace("#warning", "// #warning") + content = content.replace("__attribute__((unused))", "") + content = content.replace( + "#include ", "#include " + ) + with open(f, "w") as file: + file.write(content) + + +def main(): + name = "libdogleg" + url = "https://github.com/dkogan/libdogleg" + tag = "c971ea43088d286a3683c1039b9a85f761f7df15" + + libdogleg = Lib(name, url, tag, copy_upstream_src) + libdogleg.main() + + +if __name__ == "__main__": + main() diff --git a/upstream_utils/libdogleg_patches/0001-Convert-to-C.patch b/upstream_utils/libdogleg_patches/0001-Convert-to-C.patch new file mode 100644 index 0000000000..fa82ac501e --- /dev/null +++ b/upstream_utils/libdogleg_patches/0001-Convert-to-C.patch @@ -0,0 +1,38 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 29 Nov 2024 19:52:22 -0500 +Subject: [PATCH 1/3] Convert to C++ + +--- + dogleg.c => dogleg.cpp | 0 + dogleg.h | 8 +++++++- + 2 files changed, 7 insertions(+), 1 deletion(-) + rename dogleg.c => dogleg.cpp (100%) + +diff --git a/dogleg.c b/dogleg.cpp +similarity index 100% +rename from dogleg.c +rename to dogleg.cpp +diff --git a/dogleg.h b/dogleg.h +index 74337263a89d3448f55e125db6702ebfa7588b97..4a23b669a30642d290549bb94a1c787f079d0647 100644 +--- a/dogleg.h ++++ b/dogleg.h +@@ -126,7 +126,9 @@ typedef struct + + } dogleg_solverContext_t; + +- ++#ifdef __cplusplus ++extern "C" { ++#endif + // Fills in the given structure with the default parameter set + void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters); + +@@ -293,3 +295,7 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + int NoutlierFeatures, + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); ++ ++#ifdef __cplusplus ++} // extern "C" ++#endif diff --git a/upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch b/upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch new file mode 100644 index 0000000000..d41eeae0d7 --- /dev/null +++ b/upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch @@ -0,0 +1,79 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 29 Nov 2024 19:54:54 -0500 +Subject: [PATCH 2/3] Replace VLAs with vectors + +--- + dogleg.cpp | 17 +++++++++-------- + 1 file changed, 9 insertions(+), 8 deletions(-) + +diff --git a/dogleg.cpp b/dogleg.cpp +index 7e4259c303e21a9f4c63ba16f1bf5df131935057..9ed95f64b232f41a51fe23d72885ecadd86dc065 100644 +--- a/dogleg.cpp ++++ b/dogleg.cpp +@@ -6,6 +6,7 @@ + // Apparently I need this in MSVC to get constants + #define _USE_MATH_DEFINES + ++#include + #include + #include + #include +@@ -1907,7 +1908,7 @@ static bool getOutliernessFactors_dense( // output + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle +- double A[featureSize*(featureSize+1)/2]; ++ std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; ix[i_measurement], +- A, featureSize, *scale); ++ A.data(), featureSize, *scale); + } + + result = true; +@@ -2008,7 +2009,7 @@ static bool getOutliernessFactors_sparse( // output + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle +- double A[featureSize*(featureSize+1)/2]; ++ std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; ix[i_measurement], +- A, featureSize, *scale); ++ A.data(), featureSize, *scale); + } + + result = true; +@@ -2212,8 +2213,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + + // This is Jt because cholmod thinks in terms of col-first instead of + // row-first +- int Jt_p[featureSize+1]; +- int Jt_i[NstateActive*featureSize]; ++ std::vector Jt_p(featureSize+1); ++ std::vector Jt_i(NstateActive*featureSize); + for(int i=0; i<=featureSize; i++) + { + Jt_p[i] = i*NstateActive; +@@ -2224,8 +2225,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + cholmod_sparse Jt_query_sparse = {.nrow = ctx->Nstate, + .ncol = featureSize, + .nzmax = NstateActive*featureSize, +- .p = (void*)Jt_p, +- .i = (void*)Jt_i, ++ .p = (void*)Jt_p.data(), ++ .i = (void*)Jt_i.data(), + .x = (double*)JqueryFeature, + .sorted = 1, + .packed = 1, diff --git a/upstream_utils/libdogleg_patches/0003-Fix-C-build-errors.patch b/upstream_utils/libdogleg_patches/0003-Fix-C-build-errors.patch new file mode 100644 index 0000000000..c0ab181158 --- /dev/null +++ b/upstream_utils/libdogleg_patches/0003-Fix-C-build-errors.patch @@ -0,0 +1,324 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Sun, 1 Dec 2024 00:28:14 -0500 +Subject: [PATCH 3/3] Fix C++ build errors + +--- + dogleg.cpp | 110 +++++++++++++++++++++++++++++++---------------------- + 1 file changed, 65 insertions(+), 45 deletions(-) + +diff --git a/dogleg.cpp b/dogleg.cpp +index 9ed95f64b232f41a51fe23d72885ecadd86dc065..c93f696a9427f0644b047308d2d99302cd246462 100644 +--- a/dogleg.cpp ++++ b/dogleg.cpp +@@ -327,9 +327,9 @@ void _dogleg_testGradient(unsigned int var, const double* p0, + { + int is_sparse = NJnnz > 0; + +- double* x0 = malloc(Nmeas * sizeof(double)); +- double* x = malloc(Nmeas * sizeof(double)); +- double* p = malloc(Nstate * sizeof(double)); ++ double* x0 = static_cast(malloc(Nmeas * sizeof(double))); ++ double* x = static_cast(malloc(Nmeas * sizeof(double))); ++ double* p = static_cast(malloc(Nstate * sizeof(double))); + ASSERT(x0); + ASSERT(x); + ASSERT(p); +@@ -376,8 +376,8 @@ void _dogleg_testGradient(unsigned int var, const double* p0, + } + else + { +- J_dense = malloc( Nmeas * Nstate * sizeof(J_dense[0]) ); +- J_dense0 = malloc( Nmeas * Nstate * sizeof(J_dense[0]) ); ++ J_dense = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); ++ J_dense0 = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); + + dogleg_callback_dense_t* f_dense = (dogleg_callback_dense_t*)f; + p[var] -= GRADTEST_DELTA/2.0; +@@ -487,11 +487,13 @@ static void computeCauchyUpdate(dogleg_operatingPoint_t* point, + + // LAPACK prototypes for a packed cholesky factorization and a linear solve + // using that factorization, respectively ++extern "C" { + int dpptrf_(char* uplo, int* n, double* ap, + int* info, int uplo_len); + int dpptrs_(char* uplo, int* n, int* nrhs, + double* ap, double* b, int* ldb, int* info, + int uplo_len); ++} + + + void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +@@ -538,8 +540,8 @@ void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solve + if(ctx->factorization_dense == NULL) + { + // Need to store symmetric JtJ, so I only need one triangle of it +- ctx->factorization_dense = malloc( ctx->Nstate * (ctx->Nstate+1) / 2 * +- sizeof( ctx->factorization_dense[0])); ++ ctx->factorization_dense = static_cast(malloc( ctx->Nstate * (ctx->Nstate+1) / 2 * ++ sizeof( ctx->factorization_dense[0]))); + ASSERT(ctx->factorization_dense); + } + +@@ -582,7 +584,9 @@ void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solve + + + int info; +- dpptrf_(&(char){'L'}, &(int){ctx->Nstate}, ctx->factorization_dense, ++ char uplo = 'L'; ++ int Nstate_copy = ctx->Nstate; ++ dpptrf_(&uplo, &Nstate_copy, ctx->factorization_dense, + &info, 1); + ASSERT(info >= 0); // we MUST either succeed or see complain of singular + // JtJ +@@ -611,10 +615,10 @@ static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solv + if( ctx->is_sparse ) + { + // solve JtJ*updateGN = Jt*x. Gauss-Newton step is then -updateGN +- cholmod_dense Jt_x_dense = {.nrow = ctx->Nstate, ++ cholmod_dense Jt_x_dense = {.nrow = static_cast(ctx->Nstate), + .ncol = 1, +- .nzmax = ctx->Nstate, +- .d = ctx->Nstate, ++ .nzmax = static_cast(ctx->Nstate), ++ .d = static_cast(ctx->Nstate), + .x = point->Jt_x, + .xtype = CHOLMOD_REAL, + .dtype = CHOLMOD_DOUBLE}; +@@ -626,18 +630,22 @@ static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solv + ctx->factorization, + &Jt_x_dense, + &ctx->common); +- vec_negate(point->updateGN_cholmoddense->x, ++ vec_negate(static_cast(point->updateGN_cholmoddense->x), + ctx->Nstate); // should be more efficient than this later + +- point->updateGN_lensq = norm2(point->updateGN_cholmoddense->x, ctx->Nstate); ++ point->updateGN_lensq = norm2(static_cast(point->updateGN_cholmoddense->x), ctx->Nstate); + } + else + { + memcpy( point->updateGN_dense, point->Jt_x, ctx->Nstate * sizeof(point->updateGN_dense[0])); + int info; +- dpptrs_(&(char){'L'}, &(int){ctx->Nstate}, &(int){1}, ++ char uplo = 'L'; ++ int nhrs = 1; ++ int Nstate_copy = ctx->Nstate; ++ int Nstate_copy2 = ctx->Nstate; ++ dpptrs_(&uplo, &Nstate_copy, &nhrs, + ctx->factorization_dense, +- point->updateGN_dense, &(int){ctx->Nstate}, &info, 1); ++ point->updateGN_dense, &Nstate_copy2, &info, 1); + vec_negate(point->updateGN_dense, + ctx->Nstate); // should be more efficient than this later + +@@ -677,7 +685,7 @@ static void computeInterpolatedUpdate(double* update_dogleg, + double dsq = trustregion*trustregion; + double norm2a = point->updateCauchy_lensq; + const double* a = point->updateCauchy; +- const double* b = ctx->is_sparse ? point->updateGN_cholmoddense->x : point->updateGN_dense; ++ const double* b = ctx->is_sparse ? static_cast(point->updateGN_cholmoddense->x) : point->updateGN_dense; + + double l2 = 0.0; + double neg_c = 0.0; +@@ -1029,7 +1037,7 @@ dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements, + { + int is_sparse = NJnnz > 0; + +- dogleg_operatingPoint_t* point = malloc(sizeof(dogleg_operatingPoint_t)); ++ dogleg_operatingPoint_t* point = static_cast(malloc(sizeof(dogleg_operatingPoint_t))); + ASSERT(point != NULL); + + +@@ -1042,7 +1050,7 @@ dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements, + if(!is_sparse) + Npool += numMeasurements*Nstate + Nstate; + +- double* pool = malloc( Npool * sizeof(double) ); ++ double* pool = static_cast(malloc( Npool * sizeof(double) )); + ASSERT( pool != NULL ); + + point->p = &pool[0]; +@@ -1170,7 +1178,7 @@ static double _dogleg_optimize(double* p, unsigned int Nstate, + int is_sparse = NJnnz > 0; + + +- dogleg_solverContext_t* ctx = malloc(sizeof(dogleg_solverContext_t)); ++ dogleg_solverContext_t* ctx = static_cast(malloc(sizeof(dogleg_solverContext_t))); + ctx->f = f; + ctx->cookie = cookie; + ctx->factorization = NULL; +@@ -1294,10 +1302,13 @@ static bool pseudoinverse_J_dense(// output + memcpy(out, + &point->J_dense[i_meas0*ctx->Nstate], + NmeasInChunk*ctx->Nstate*sizeof(double)); +- dpptrs_(&(char){'L'}, &(int){ctx->Nstate}, &NmeasInChunk, ++ char uplo = 'L'; ++ int Nstate_copy = ctx->Nstate; ++ int Nstate_copy2 = ctx->Nstate; ++ dpptrs_(&uplo, &Nstate_copy, &NmeasInChunk, + ctx->factorization_dense, + out, +- &(int){ctx->Nstate}, &info, 1); ++ &Nstate_copy2, &info, 1); + return info==0; + } + +@@ -1873,11 +1884,12 @@ static bool getOutliernessFactors_dense( // output + int Nmeasurements = ctx->Nmeasurements; + bool result = false; + +- double* invJtJ_Jt = malloc(Nstate*chunk_size*sizeof(double)); ++ double* invJtJ_Jt = static_cast(malloc(Nstate*chunk_size*sizeof(double))); + if(invJtJ_Jt == NULL) + { + SAY("Couldn't allocate invJtJ_Jt!"); +- goto done; ++ free(invJtJ_Jt); ++ return result; + } + + getOutliernessScale(scale, +@@ -1895,7 +1907,8 @@ static bool getOutliernessFactors_dense( // output + if(!pinvresult) + { + SAY("Couldn't compute pinv!"); +- goto done; ++ free(invJtJ_Jt); ++ return result; + } + i_measurement_valid_chunk_start = i_measurement; + i_measurement_valid_chunk_last = i_measurement+chunk_size-1; +@@ -1926,7 +1939,6 @@ static bool getOutliernessFactors_dense( // output + } + + result = true; +- done: + free(invJtJ_Jt); + return result; + } +@@ -1975,7 +1987,9 @@ static bool getOutliernessFactors_sparse( // output + if(!Jt_chunk) + { + SAY("Couldn't allocate Jt_chunk!"); +- goto done; ++ if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); ++ if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); ++ return result; + } + + getOutliernessScale(scale, +@@ -1995,7 +2009,9 @@ static bool getOutliernessFactors_sparse( // output + if(invJtJ_Jt == NULL) + { + SAY("Couldn't compute pinv!"); +- goto done; ++ if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); ++ if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); ++ return result; + } + + i_measurement_valid_chunk_start = i_measurement; +@@ -2032,7 +2048,6 @@ static bool getOutliernessFactors_sparse( // output + } + + result = true; +- done: + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; +@@ -2222,18 +2237,18 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + for(int j=0; jNstate, +- .ncol = featureSize, +- .nzmax = NstateActive*featureSize, +- .p = (void*)Jt_p.data(), +- .i = (void*)Jt_i.data(), +- .x = (double*)JqueryFeature, +- .sorted = 1, +- .packed = 1, ++ cholmod_sparse Jt_query_sparse = {.nrow = static_cast(ctx->Nstate), ++ .ncol = static_cast(featureSize), ++ .nzmax = static_cast(NstateActive*featureSize), ++ .p = Jt_p.data(), ++ .i = Jt_i.data(), ++ .x = const_cast(JqueryFeature), + .stype = 0, // NOT symmetric + .itype = CHOLMOD_INT, + .xtype = CHOLMOD_REAL, +- .dtype = CHOLMOD_DOUBLE}; ++ .dtype = CHOLMOD_DOUBLE, ++ .sorted = 1, ++ .packed = 1}; + + // Really shouldn't need to do this every time. In fact I probably don't need + // to do it at all, since this will have been done by the solver during the +@@ -2408,18 +2423,22 @@ bool dogleg_markOutliers(// output, input + + bool markedAny = false; + +- double* factors = malloc(Nfeatures * sizeof(double)); ++ double* factors = static_cast(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); +- goto done; ++ free(factors); ++ return markedAny; + } + + if(!dogleg_getOutliernessFactors(factors, scale, + featureSize, Nfeatures, + *Noutliers, + point, ctx)) +- goto done; ++ { ++ free(factors); ++ return markedAny; ++ } + + // I have my list of POTENTIAL outliers (any that have factor > 1.0). I + // check to see how much confidence I would lose if I were to throw out any +@@ -2427,7 +2446,10 @@ bool dogleg_markOutliers(// output, input + // is acceptable + double confidence0 = getConfidence(-1); + if( confidence0 < 0.0 ) +- goto done; ++ { ++ free(factors); ++ return markedAny; ++ } + + SAY_IF_VERBOSE("Initial confidence: %g", confidence0); + +@@ -2466,7 +2488,6 @@ bool dogleg_markOutliers(// output, input + } + } + +- done: + free(factors); + return markedAny; + } +@@ -2490,11 +2511,11 @@ void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude), + if(featureSize <= 1) + featureSize = 1; + +- double* factors = malloc(Nfeatures * sizeof(double)); ++ double* factors = static_cast(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); +- goto done; ++ free(factors); + } + + dogleg_getOutliernessFactors(factors, scale, +@@ -2516,6 +2537,5 @@ void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude), + rot_confidence_drop_relative); + } + +- done: + free(factors); + } diff --git a/upstream_utils/mrcal.py b/upstream_utils/mrcal.py new file mode 100755 index 0000000000..be6f57d7c6 --- /dev/null +++ b/upstream_utils/mrcal.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import Lib, walk_cwd_and_copy_if + + +def copy_upstream_src(wpilib_root): + wpical = os.path.join(wpilib_root, "wpical") + + # Delete old install + for d in [ + "src/main/native/thirdparty/mrcal/src", + "src/main/native/thirdparty/mrcal/include", + ]: + shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + + files = walk_cwd_and_copy_if( + lambda dp, f: (f.endswith(".h") or f.endswith(".hh")) + and not f.endswith("heap.h") + and not f.endswith("stereo-matching-libelas.h") + and not dp.startswith(os.path.join(".", "test")), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), + ) + files = files + walk_cwd_and_copy_if( + lambda dp, f: ( + f.endswith(".c") + or f.endswith(".cc") + or f.endswith(".cpp") + or f.endswith(".pl") + ) + and not f.endswith("heap.cc") + and not f.endswith("mrcal-pywrap.c") + and not f.endswith("image.c") + and not f.endswith("stereo.c") + and not f.endswith("stereo-matching-libelas.cc") + and not f.endswith("uncertainty.c") + and not f.endswith("traverse-sensor-links.c") + and not dp.startswith(os.path.join(".", "doc")) + and not dp.startswith(os.path.join(".", "test")), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/src"), + ) + + for f in files: + with open(f) as file: + content = file.read() + content = content.replace("#warning", "// #warning") + content = content.replace('__attribute__ ((visibility ("hidden")))', "") + content = content.replace("__attribute__((unused))", "") + with open(f, "w") as file: + file.write(content) + + +def main(): + name = "mrcal" + url = "https://github.com/dkogan/mrcal" + tag = "662a539d3cbba4948c31d06a780569173b3fb6e6" + + mrcal = Lib(name, url, tag, copy_upstream_src) + mrcal.main() + + +if __name__ == "__main__": + main() diff --git a/upstream_utils/mrcal_java.py b/upstream_utils/mrcal_java.py new file mode 100755 index 0000000000..5fdd639b16 --- /dev/null +++ b/upstream_utils/mrcal_java.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import Lib, walk_cwd_and_copy_if + + +def delete_lines_by_range(file_path, start_line, end_line): + # Read all lines from the file + with open(file_path, "r") as file: + lines = file.readlines() + + # Filter out lines that are within the specified range + filtered_lines = [ + line + for i, line in enumerate(lines, start=1) + if not (start_line <= i <= end_line) + ] + + # Write the remaining lines back to the file + with open(file_path, "w") as file: + file.writelines(filtered_lines) + + +def copy_upstream_src(wpilib_root): + wpical = os.path.join(wpilib_root, "wpical") + + # Delete old install + for d in [ + "src/main/native/thirdparty/mrcal_java/src", + "src/main/native/thirdparty/mrcal_java/include", + ]: + shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + + os.chdir("src") + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith("mrcal_wrapper.h"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/include"), + ) + + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith("mrcal_wrapper.cpp"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/src"), + ) + + for f in files: + with open(f) as file: + content = file.read() + content = content.replace("#include ", "") + content = content.replace( + "// mrcal_point3_t *c_observations_point_pool = observations_point;", + "mrcal_point3_t *c_observations_point_pool = observations_point;", + ) + content = content.replace( + "// mrcal_observation_point_triangulated_t *observations_point_triangulated =", + "mrcal_observation_point_triangulated_t *observations_point_triangulated = NULL;", + ) + content = content.replace( + "// observations_point_triangulated,", + "observations_point_triangulated,", + ) + content = content.replace( + "// 0, // hard-coded to 0", "0, // hard-coded to 0" + ) + content = content.replace( + "// observations_point_triangulated, -1,", + "observations_point_triangulated, -1,", + ) + content = content.replace( + "c_observations_board_pool, &mrcal_lensmodel, c_imagersizes,", + "c_observations_board_pool, c_observations_point_pool, &mrcal_lensmodel, c_imagersizes,", + ) + with open(f, "w") as file: + file.write(content) + + +def main(): + name = "mrcal_java" + url = "https://github.com/PhotonVision/mrcal-java" + tag = "5f9d3168ccf1ecdfca48da13ea07fffa47f95d00" + + mrcal_java = Lib(name, url, tag, copy_upstream_src) + mrcal_java.main() + + +if __name__ == "__main__": + main() diff --git a/upstream_utils/mrcal_java_patches/0001-Support-new-mrcal-version.patch b/upstream_utils/mrcal_java_patches/0001-Support-new-mrcal-version.patch new file mode 100644 index 0000000000..26a7777b6c --- /dev/null +++ b/upstream_utils/mrcal_java_patches/0001-Support-new-mrcal-version.patch @@ -0,0 +1,22 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Sat, 30 Nov 2024 17:52:37 -0500 +Subject: [PATCH] Support new mrcal version + +--- + src/mrcal_wrapper.cpp | 2 +- + 1 file changed, 1 insertion(+), 1 deletion(-) + +diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp +index 569f5928ba0b389e40476cbfc224a0c09ecd5814..3a9a5519e670b0c867281f1e05aa2046b938fa8d 100644 +--- a/src/mrcal_wrapper.cpp ++++ b/src/mrcal_wrapper.cpp +@@ -212,7 +212,7 @@ static std::unique_ptr mrcal_calibrate( + std::vector residuals = {c_x_final, c_x_final + Nmeasurements}; + return std::make_unique( + true, intrinsics, stats.rms_reproj_error__pixels, residuals, +- calobject_warp, stats.Noutliers); ++ calobject_warp, stats.Noutliers_board); + } + + struct MrcalSolveOptions { diff --git a/upstream_utils/mrcal_patches/0001-Fix-MSVC-build-errors.patch b/upstream_utils/mrcal_patches/0001-Fix-MSVC-build-errors.patch new file mode 100644 index 0000000000..cc4a3b3c6a --- /dev/null +++ b/upstream_utils/mrcal_patches/0001-Fix-MSVC-build-errors.patch @@ -0,0 +1,155 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Mon, 11 Nov 2024 00:15:05 -0500 +Subject: [PATCH 1/8] Fix MSVC build errors + +--- + autodiff.hh | 4 ++-- + mrcal-internal.h | 22 +++++++++++----------- + mrcal-types.h | 26 ++++++++++---------------- + mrcal.c | 9 --------- + 4 files changed, 23 insertions(+), 38 deletions(-) + +diff --git a/autodiff.hh b/autodiff.hh +index b6f217354c9de45493d50f776082fe2151330a1d..37aa8c5a048a538ccdf500a43bbd8d788c953a05 100644 +--- a/autodiff.hh ++++ b/autodiff.hh +@@ -26,7 +26,7 @@ template + struct val_withgrad_t + { + double x; +- double j[NGRAD]; ++ double j[NGRAD == 0 ? 1 : NGRAD]; + + __attribute__ ((visibility ("hidden"))) + val_withgrad_t(double _x = 0.0) : x(_x) +@@ -281,7 +281,7 @@ struct val_withgrad_t + template + struct vec_withgrad_t + { +- val_withgrad_t v[NVEC]; ++ val_withgrad_t v[NVEC == 0 ? 1 : NVEC]; + + vec_withgrad_t() {} + +diff --git a/mrcal-internal.h b/mrcal-internal.h +index 4ec6acadb5fc92ec264fb460e1fcc882dd3face3..8fc2857758ef7763cc1ef5b79a495aec7b7bc1bd 100644 +--- a/mrcal-internal.h ++++ b/mrcal-internal.h +@@ -12,16 +12,16 @@ + // wrapper only + + // These models have no precomputed data +-typedef struct {} mrcal_LENSMODEL_PINHOLE__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_LONLAT__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_LATLON__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV4__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV5__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV8__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV12__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_CAHVOR__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_CAHVORE__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_CAHVORE__precomputed_t; + + // The splined stereographic models configuration parameters can be used to + // compute the segment size. I cache this computation +@@ -38,7 +38,7 @@ typedef struct + union + { + #define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed; +- MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT); ++ MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT) + #undef PRECOMPUTED_STRUCT + }; + } mrcal_projection_precomputed_t; +diff --git a/mrcal-types.h b/mrcal-types.h +index e5cf6c637f8f2bf9e4bbe4ad443661159c511887..7e43319b882c3e29eb3429f95c77fc8bd664ce1a 100644 +--- a/mrcal-types.h ++++ b/mrcal-types.h +@@ -45,24 +45,18 @@ + + + // parametric models have no extra configuration +-typedef struct {} mrcal_LENSMODEL_PINHOLE__config_t; +-typedef struct {} mrcal_LENSMODEL_STEREOGRAPHIC__config_t; +-typedef struct {} mrcal_LENSMODEL_LONLAT__config_t; +-typedef struct {} mrcal_LENSMODEL_LATLON__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV4__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV5__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV8__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV12__config_t; +-typedef struct {} mrcal_LENSMODEL_CAHVOR__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__config_t; + + #define _MRCAL_ITEM_DEFINE_ELEMENT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) type name bitfield; + +-#ifndef __cplusplus +-// This barfs with g++ 4.8, so I disable it for C++ in general. Checking it for +-// C code is sufficient +-_Static_assert(sizeof(uint16_t) == sizeof(unsigned short int), "I need a short to be 16-bit. Py_BuildValue doesn't let me just specify that. H means 'unsigned short'"); +-#endif +- + // Configuration for CAHVORE. These are given as an an + // "X macro": https://en.wikipedia.org/wiki/X_Macro + #define MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_, cookie) \ +@@ -128,7 +122,7 @@ typedef struct + union + { + #define CONFIG_STRUCT(s,n) mrcal_ ##s##__config_t s##__config; +- MRCAL_LENSMODEL_LIST(CONFIG_STRUCT); ++ MRCAL_LENSMODEL_LIST(CONFIG_STRUCT) + #undef CONFIG_STRUCT + }; + } mrcal_lensmodel_t; +diff --git a/mrcal.c b/mrcal.c +index d1195a36050e5cbde393ed033e4d5699ce52225a..85ab4e26a5ab487fc13e66ebd486bd7c00480c05 100644 +--- a/mrcal.c ++++ b/mrcal.c +@@ -2674,8 +2674,6 @@ void project( // out + if(!camera_at_identity) + { + // make sure I can pass mrcal_pose_t.r as an rt[] transformation +- _Static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure"); +- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure"); + mrcal_compose_rt( _joint_rt, + gg._d_rj_rc, gg._d_rj_rf, + gg._d_tj_rc, gg._d_tj_tf, +@@ -3465,8 +3463,6 @@ void mrcal_pack_solver_state_vector( // out, in + lensmodel, problem_selections, + Ncameras_intrinsics ); + +- _Static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure"); +- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure"); + if( problem_selections.do_optimize_extrinsics ) + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + { +@@ -3715,11 +3711,6 @@ void mrcal_unpack_solver_state_vector( // out, in + + if( problem_selections.do_optimize_extrinsics ) + { +- _Static_assert( offsetof(mrcal_pose_t, r) == 0, +- "mrcal_pose_t has expected structure"); +- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), +- "mrcal_pose_t has expected structure"); +- + mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&b[i_state]); + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &b[i_state] ); diff --git a/upstream_utils/mrcal_patches/0002-Make-mrcal-functions-extern-C.patch b/upstream_utils/mrcal_patches/0002-Make-mrcal-functions-extern-C.patch new file mode 100644 index 0000000000..7f5afcad1d --- /dev/null +++ b/upstream_utils/mrcal_patches/0002-Make-mrcal-functions-extern-C.patch @@ -0,0 +1,34 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 22 Nov 2024 09:54:00 -0500 +Subject: [PATCH 2/8] Make mrcal functions extern "C" + +--- + mrcal.h | 7 ++++++- + 1 file changed, 6 insertions(+), 1 deletion(-) + +diff --git a/mrcal.h b/mrcal.h +index 1eef334a79597cfae61f8111ab97ca04f02b83b2..488ddfe72b3407b3fcb7c3d9de43d1a07433744d 100644 +--- a/mrcal.h ++++ b/mrcal.h +@@ -20,7 +20,9 @@ + //////////////////////////////////////////////////////////////////////////////// + //////////////////// Lens models + //////////////////////////////////////////////////////////////////////////////// +- ++#ifdef __cplusplus ++extern "C" { ++#endif + // Return an array of strings listing all the available lens models + // + // These are all "unconfigured" strings that use "..." placeholders for any +@@ -846,6 +848,9 @@ void mrcal_free_cameramodel(mrcal_cameramodel_t** cameramodel); + bool mrcal_write_cameramodel_file(const char* filename, + const mrcal_cameramodel_t* cameramodel); + ++#ifdef __cplusplus ++} // extern "C" ++#endif + #define DECLARE_mrcal_apply_color_map(T,Tname) \ + bool mrcal_apply_color_map_##Tname( \ + mrcal_image_bgr_t* out, \ diff --git a/upstream_utils/mrcal_patches/0003-Make-mrcal-C.patch b/upstream_utils/mrcal_patches/0003-Make-mrcal-C.patch new file mode 100644 index 0000000000..4180945cd3 --- /dev/null +++ b/upstream_utils/mrcal_patches/0003-Make-mrcal-C.patch @@ -0,0 +1,14 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 22 Nov 2024 09:55:22 -0500 +Subject: [PATCH 3/8] Make mrcal C++ + +--- + mrcal.c => mrcal.cpp | 0 + 1 file changed, 0 insertions(+), 0 deletions(-) + rename mrcal.c => mrcal.cpp (100%) + +diff --git a/mrcal.c b/mrcal.cpp +similarity index 100% +rename from mrcal.c +rename to mrcal.cpp diff --git a/upstream_utils/mrcal_patches/0004-Replace-VLAs-with-vectors.patch b/upstream_utils/mrcal_patches/0004-Replace-VLAs-with-vectors.patch new file mode 100644 index 0000000000..454c796e9f --- /dev/null +++ b/upstream_utils/mrcal_patches/0004-Replace-VLAs-with-vectors.patch @@ -0,0 +1,284 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 29 Nov 2024 22:56:02 -0500 +Subject: [PATCH 4/8] Replace VLAs with vectors + +--- + mrcal.cpp | 101 +++++++++++++++++++++++++++--------------------------- + 1 file changed, 51 insertions(+), 50 deletions(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index 85ab4e26a5ab487fc13e66ebd486bd7c00480c05..5f28f3303da4b4669af896587427b2ad3ad4b7e6 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -7,6 +7,7 @@ + // http://www.apache.org/licenses/LICENSE-2.0 + + // Apparently I need this in MSVC to get constants ++#include + #define _USE_MATH_DEFINES + + #include +@@ -28,7 +29,7 @@ + + #define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0) + +- ++#define restrict __restrict + + // Returns a static string, using "..." as a placeholder for any configuration + // values +@@ -1141,7 +1142,7 @@ void project_cahvor( // outputs + double s__dmu_dalphabeta__domega_dalphabeta = dmu_dtau * s__dtau_dalphabeta__domega_dalphabeta; + + double dpdistorted_dpcam[3*3] = {}; +- double dpdistorted_ddistortion[3*NdistortionParams]; ++ std::vector dpdistorted_ddistortion(3*NdistortionParams); + + for(int i=0; i<3; i++) + { +@@ -2918,7 +2919,7 @@ bool _mrcal_project_internal( // out + .t = p[i]}; + + // simple non-intrinsics-gradient path +- double dq_dintrinsics_pool_double[Ngradients]; ++ std::vector dq_dintrinsics_pool_double(Ngradients); + int dq_dintrinsics_pool_int [1]; + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; +@@ -2926,7 +2927,7 @@ bool _mrcal_project_internal( // out + + project( &q[i], + +- dq_dintrinsics_pool_double, ++ dq_dintrinsics_pool_double.data(), + dq_dintrinsics_pool_int, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + +@@ -3943,21 +3944,21 @@ bool mrcal_corresponding_icam_extrinsics(// out + return false; + } + +- int icam_map_to_extrinsics[Ncameras_intrinsics]; +- int icam_map_to_intrinsics[Ncameras_extrinsics+1]; ++ std::vector icam_map_to_extrinsics(Ncameras_intrinsics); ++ std::vector icam_map_to_intrinsics(Ncameras_extrinsics+1); + for(int i=0; iNcameras_intrinsics][ctx->Nintrinsics]; +- mrcal_pose_t camera_rt[ctx->Ncameras_extrinsics]; ++ std::vector> intrinsics_all(ctx->Ncameras_intrinsics, std::vector(ctx->Nintrinsics)); ++ std::vector camera_rt(ctx->Ncameras_extrinsics); + + mrcal_calobject_warp_t calobject_warp_local = {}; + const int i_var_calobject_warp = +@@ -4669,12 +4670,12 @@ void optimizer_callback(// input state + + // these are computed in respect to the real-unit parameters, + // NOT the unit-scale parameters used by the optimizer +- mrcal_point3_t dq_drcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point3_t dq_dtcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point3_t dq_drframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point3_t dq_dtframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_calobject_warp_t dq_dcalobject_warp[ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point2_t q_hypothesis [ctx->calibration_object_width_n*ctx->calibration_object_height_n]; ++ std::vector dq_drcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_drframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector q_hypothesis (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + // I get the intrinsics gradients in separate arrays, possibly sparsely. + // All the data lives in dq_dintrinsics_pool_double[], with the other data + // indicating the meaning of the values in the pool. +@@ -4686,35 +4687,35 @@ void optimizer_callback(// input state + // this case explicitly here. I store dx/dfx and dy/dfy; no cross terms + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + +- double dq_dintrinsics_pool_double[ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients]; +- int dq_dintrinsics_pool_int [ctx->calibration_object_width_n*ctx->calibration_object_height_n]; ++ std::vector dq_dintrinsics_pool_double(ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients); ++ std::vector dq_dintrinsics_pool_int (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; + + int splined_intrinsics_grad_irun = 0; + +- project(q_hypothesis, ++ project(q_hypothesis.data(), + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +- dq_dintrinsics_pool_double : NULL, ++ dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +- dq_dintrinsics_pool_int : NULL, ++ dq_dintrinsics_pool_int.data() : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + ctx->problem_selections.do_optimize_extrinsics ? +- (mrcal_point3_t*)dq_drcamera : NULL, ++ dq_drcamera.data() : NULL, + ctx->problem_selections.do_optimize_extrinsics ? +- (mrcal_point3_t*)dq_dtcamera : NULL, ++ dq_dtcamera.data() : NULL, + ctx->problem_selections.do_optimize_frames ? +- (mrcal_point3_t*)dq_drframe : NULL, ++ dq_drframe.data() : NULL, + ctx->problem_selections.do_optimize_frames ? +- (mrcal_point3_t*)dq_dtframe : NULL, ++ dq_dtframe.data() : NULL, + has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ? +- (mrcal_calobject_warp_t*)dq_dcalobject_warp : NULL, ++ dq_dcalobject_warp.data() : NULL, + + // input +- intrinsics_all[icam_intrinsics], ++ intrinsics_all[icam_intrinsics].data(), + &camera_rt[icam_extrinsics], &frame_rt, + ctx->calobject_warp == NULL ? NULL : &calobject_warp_local, + icam_extrinsics < 0, +@@ -4800,43 +4801,43 @@ void optimizer_callback(// input state + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, +- dq_drcamera[i_pt][i_xy].xyz[0] * ++ dq_drcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_CAMERA, +- dq_drcamera[i_pt][i_xy].xyz[1] * ++ dq_drcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_CAMERA, +- dq_drcamera[i_pt][i_xy].xyz[2] * ++ dq_drcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_CAMERA); + STORE_JACOBIAN3( i_var_camera_rt + 3, +- dq_dtcamera[i_pt][i_xy].xyz[0] * ++ dq_dtcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_CAMERA, +- dq_dtcamera[i_pt][i_xy].xyz[1] * ++ dq_dtcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_CAMERA, +- dq_dtcamera[i_pt][i_xy].xyz[2] * ++ dq_dtcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_CAMERA); + } + + if( ctx->problem_selections.do_optimize_frames ) + { + STORE_JACOBIAN3( i_var_frame_rt + 0, +- dq_drframe[i_pt][i_xy].xyz[0] * ++ dq_drframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_FRAME, +- dq_drframe[i_pt][i_xy].xyz[1] * ++ dq_drframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_FRAME, +- dq_drframe[i_pt][i_xy].xyz[2] * ++ dq_drframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_FRAME); + STORE_JACOBIAN3( i_var_frame_rt + 3, +- dq_dtframe[i_pt][i_xy].xyz[0] * ++ dq_dtframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_FRAME, +- dq_dtframe[i_pt][i_xy].xyz[1] * ++ dq_dtframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_FRAME, +- dq_dtframe[i_pt][i_xy].xyz[2] * ++ dq_dtframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_FRAME); + } + + if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ) + { + STORE_JACOBIAN_N( i_var_calobject_warp, +- dq_dcalobject_warp[i_pt][i_xy].values, ++ dq_dcalobject_warp[i_pt * 2 + i_xy].values, + weight * SCALE_CALOBJECT_WARP, + MRCAL_NSTATE_CALOBJECT_WARP); + } +@@ -5061,7 +5062,7 @@ void optimizer_callback(// input state + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + + // WARNING: "compute size(dq_dintrinsics_pool_double) correctly and maybe bounds-check" +- double dq_dintrinsics_pool_double[Ngradients]; ++ std::vector dq_dintrinsics_pool_double(Ngradients); + // used for LENSMODEL_SPLINED_STEREOGRAPHIC only, but getting rid of + // this in other cases isn't worth the trouble + int dq_dintrinsics_pool_int [1]; +@@ -5081,7 +5082,7 @@ void optimizer_callback(// input state + project(&q_hypothesis, + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +- dq_dintrinsics_pool_double : NULL, ++ dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_int : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, +@@ -5095,7 +5096,7 @@ void optimizer_callback(// input state + NULL, + + // input +- intrinsics_all[icam_intrinsics], ++ intrinsics_all[icam_intrinsics].data(), + &camera_rt[icam_extrinsics], + + // I only have the point position, so the 'rt' memory +@@ -6590,8 +6591,8 @@ mrcal_optimize( // out + + // WARNING: is it reasonable to put this on the stack? Can I use + // b_packed_final for this? +- double packed_state[Nstate]; +- pack_solver_state(packed_state, ++ std::vector packed_state(Nstate); ++ pack_solver_state(packed_state.data(), + lensmodel, intrinsics, + extrinsics_fromref, + frames_toref, +@@ -6628,7 +6629,7 @@ mrcal_optimize( // out + if(solver_context != NULL) + dogleg_freeContext(&solver_context); + +- norm2_error = dogleg_optimize2(packed_state, ++ norm2_error = dogleg_optimize2(packed_state.data(), + Nstate, ctx.Nmeasurements, ctx.N_j_nonzero, + (dogleg_callback_t*)&optimizer_callback, &ctx, + &dogleg_parameters, +@@ -6683,7 +6684,7 @@ mrcal_optimize( // out + frames_toref, // Nframes of these + points, // Npoints of these + calobject_warp, +- packed_state, ++ packed_state.data(), + lensmodel, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, +@@ -6793,7 +6794,7 @@ mrcal_optimize( // out + } + else + for(int ivar=0; ivar +Date: Fri, 29 Nov 2024 22:56:22 -0500 +Subject: [PATCH 5/8] Use immediately invoked lambda + +--- + mrcal.cpp | 4 ++-- + 1 file changed, 2 insertions(+), 2 deletions(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index 5f28f3303da4b4669af896587427b2ad3ad4b7e6..e048bef1d1b5addfd137d3c492108c36e56eda7f 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -6668,11 +6668,11 @@ mrcal_optimize( // out + solver_context->beforeStep->x, + extrinsics_fromref, + verbose) && +- ({MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again", ++ ([=]{MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again", + stats.Noutliers_board, + Nmeasurements_board, + (double)(stats.Noutliers_board * 100) / (double)Nmeasurements_board); +- true;})); ++ return true;}())); + #if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS + #warning "triangulated-solve: the above print should deal with triangulated points too" + #endif diff --git a/upstream_utils/mrcal_patches/0006-Fix-MSVC-build-errors.patch b/upstream_utils/mrcal_patches/0006-Fix-MSVC-build-errors.patch new file mode 100644 index 0000000000..c585a88637 --- /dev/null +++ b/upstream_utils/mrcal_patches/0006-Fix-MSVC-build-errors.patch @@ -0,0 +1,434 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Wed, 4 Dec 2024 00:58:00 -0500 +Subject: [PATCH 6/8] Fix MSVC build errors + +--- + minimath/minimath-extra.h | 1 + + mrcal.cpp | 57 ++++++----- + poseutils.c | 209 -------------------------------------- + 3 files changed, 32 insertions(+), 235 deletions(-) + +diff --git a/minimath/minimath-extra.h b/minimath/minimath-extra.h +index 2930a5d6aa47af29a3255342838d679032ee4562..194b0ccf22c47a8d693eee24f6f563a2d651f338 100644 +--- a/minimath/minimath-extra.h ++++ b/minimath/minimath-extra.h +@@ -5,6 +5,7 @@ + // replacement, and I'm not going to be thorough and I'm not going to add tests + // until I do that. + ++#define restrict __restrict + + // Upper triangle is stored, in the usual row-major order. + __attribute__((unused)) +diff --git a/mrcal.cpp b/mrcal.cpp +index e048bef1d1b5addfd137d3c492108c36e56eda7f..1560cd106ff5bcfe49476f785aa8108119318b3a 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -2742,11 +2742,10 @@ void project( // out + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config; + *gradient_sparse_meta = +- (gradient_sparse_meta_t) + { +- .run_side_length = config->order+1, +- .ivar_stridey = 2*config->Nx, +- .pool = &dq_dintrinsics_pool_double[ivar_pool] ++ &dq_dintrinsics_pool_double[ivar_pool], ++ static_cast(config->order+1), ++ static_cast(2*config->Nx), + }; + ivar_pool += Npoints*2 * runlen; + } +@@ -2765,10 +2764,11 @@ void project( // out + + if( calibration_object_width_n == 0 ) + { // projecting discrete points ++ mrcal_point3_t pt_ref = {}; // Unused + mrcal_point3_t p = + _propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf, + &dp_drc,&dp_dtc,&dp_drf,&dp_dtf, +- &(mrcal_point3_t){}, ++ &pt_ref, + camera_at_identity ? NULL : &gg, + Rj, d_Rj_rj, &joint_rt[3]); + _project_point( q, +@@ -3227,10 +3227,11 @@ bool _mrcal_unproject_internal( // out + // MSG("init. q=(%g,%g)", q[i].x, q[i].y); + + // initial estimate: pinhole projection ++ const mrcal_point3_t v = {.x = (q[i].x-cx)/fx, ++ .y = (q[i].y-cy)/fy, ++ .z = 1.}; + mrcal_project_stereographic( (mrcal_point2_t*)out->xyz, NULL, +- &(mrcal_point3_t){.x = (q[i].x-cx)/fx, +- .y = (q[i].y-cy)/fy, +- .z = 1.}, ++ &v, + 1, + intrinsics ); + // MSG("init. out->xyz[]=(%g,%g)", out->x, out->y); +@@ -4670,10 +4671,10 @@ void optimizer_callback(// input state + + // these are computed in respect to the real-unit parameters, + // NOT the unit-scale parameters used by the optimizer +- std::vector dq_drcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); +- std::vector dq_dtcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); +- std::vector dq_drframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); +- std::vector dq_dtframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_drcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_drframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector q_hypothesis (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + // I get the intrinsics gradients in separate arrays, possibly sparsely. +@@ -6242,7 +6243,7 @@ bool mrcal_optimizer_callback(// out + problem_selections.do_optimize_extrinsics ) ) + { + MSG("ERROR: We have triangulated points. At this time this is only allowed if we're NOT optimizing intrinsics AND if we ARE optimizing extrinsics."); +- goto done; ++ return result; + } + + if( Nobservations_board > 0 ) +@@ -6250,7 +6251,7 @@ bool mrcal_optimizer_callback(// out + if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL ) + { + MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in."); +- goto done; ++ return result; + } + } + else +@@ -6266,7 +6267,7 @@ bool mrcal_optimizer_callback(// out + !problem_selections.do_optimize_calobject_warp) + { + MSG("Not optimizing any of our variables!"); +- goto done; ++ return result; + } + + +@@ -6279,7 +6280,7 @@ bool mrcal_optimizer_callback(// out + { + MSG("The buffer passed to fill-in b_packed has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nstate*(int)sizeof(double),buffer_size_b_packed); +- goto done; ++ return result; + } + + int Nmeasurements = mrcal_num_measurements(Nobservations_board, +@@ -6312,7 +6313,7 @@ bool mrcal_optimizer_callback(// out + { + MSG("The buffer passed to fill-in x has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nmeasurements*(int)sizeof(double),buffer_size_x); +- goto done; ++ return result; + } + + const int Npoints_fromBoards = +@@ -6332,9 +6333,9 @@ bool mrcal_optimizer_callback(// out + .Npoints_fixed = Npoints_fixed, + .observations_board = observations_board, + .observations_board_pool = observations_board_pool, +- .observations_point_pool = observations_point_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, ++ .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, +@@ -6367,7 +6368,6 @@ bool mrcal_optimizer_callback(// out + + result = true; + +-done: + return result; + } + +@@ -6516,9 +6516,9 @@ mrcal_optimize( // out + .Npoints_fixed = Npoints_fixed, + .observations_board = observations_board, + .observations_board_pool = observations_board_pool, +- .observations_point_pool = observations_point_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, ++ .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, +@@ -6635,10 +6635,13 @@ mrcal_optimize( // out + &dogleg_parameters, + &solver_context); + +- if(norm2_error < 0) ++ if(norm2_error < 0) { + // libdogleg barfed. I quit out +- goto done; ++ if(solver_context != NULL) ++ dogleg_freeContext(&solver_context); + ++ return stats; ++ } + #if 0 + // Not using dogleg_markOutliers() (yet...) + +@@ -6810,7 +6813,6 @@ mrcal_optimize( // out + if(x_final) + memcpy(x_final, solver_context->beforeStep->x, ctx.Nmeasurements*sizeof(double)); + +- done: + if(solver_context != NULL) + dogleg_freeContext(&solver_context); + +@@ -6834,7 +6836,9 @@ bool mrcal_write_cameramodel_file(const char* filename, + { + MSG("Couldn't construct lensmodel string. Unconfigured string: '%s'", + mrcal_lensmodel_name_unconfigured(&cameramodel->lensmodel)); +- goto done; ++ if(fp != NULL) ++ fclose(fp); ++ return result; + } + + int Nparams = mrcal_lensmodel_num_params(&cameramodel->lensmodel); +@@ -6842,7 +6846,9 @@ bool mrcal_write_cameramodel_file(const char* filename, + { + MSG("Couldn't get valid Nparams from lensmodel string '%s'", + lensmodel_string); +- goto done; ++ if(fp != NULL) ++ fclose(fp); ++ return result;; + } + + fprintf(fp, "{\n"); +@@ -6865,7 +6871,6 @@ bool mrcal_write_cameramodel_file(const char* filename, + fprintf(fp,"}\n"); + result = true; + +- done: + if(fp != NULL) + fclose(fp); + return result; +diff --git a/poseutils.c b/poseutils.c +index 80d2b0d88dd5ad264bb579a7062b051d9972be74..59f7738477127829b87f490b5a2337c2378c2309 100644 +--- a/poseutils.c ++++ b/poseutils.c +@@ -1079,215 +1079,6 @@ void mrcal_r_from_R_full( // output + } + } + +-// LAPACK SVD function +-int dgesdd_(char* jobz, +- int* m, +- int* n, +- double* a, +- int* lda, +- double* s, +- double* u, +- int* ldu, +- double* vt, +- int* ldvt, +- double* work, +- int* lwork, +- int* iwork, +- int* info, +- int jobz_len); +- +-// This is functionally identical to mrcal.align_procrustes_vectors_R01(). It +-// should replace that function to provide a C implementation for mrcal users +-// +-// This solves: +-// https://en.wikipedia.org/wiki/Orthogonal_Procrustes_problem +-// See the mrcal sources for implementation details +-static +-bool _align_procrustes_vectors_R01(// out +- double* R01, +- // in +- const int N, +- // (N,3) arrays +- const double* p0, +- const double* p1, +- // (3,) array; may be NULL +- const double* pmean0, +- const double* pmean1, +- +- // (N,) array; may be NULL to use an even +- // weighting +- const double* weights) +-{ +- double M[9] = {}; +- +- double _pmean0[3] = {}; +- double _pmean1[3] = {}; +- if(pmean0 == NULL) pmean0 = _pmean0; +- if(pmean1 == NULL) pmean1 = _pmean1; +- +- if(weights == NULL) +- for(int i=0; i At = V Ut. And the results it gives back to me +- // are transposed too. So I give it At. The "U" it gives me back is actually +- // Vt and the Vt is actually U +- dgesdd_("A", +- (int[]){3}, (int[]){3}, +- M, (int[]){3}, +- S, +- Vt,(int[]){3}, +- U, (int[]){3}, +- &lwork_query, +- (int[]){-1}, // query the optimal lwork +- iwork, +- &info, +- 1); +- if(info != 0) +- { +- // secret value to indicate that this is a fatal error. Needed for the +- // Python layer +- R01[0] = 1.; +- return false; +- } +- +- double work[(int)lwork_query]; +- +- dgesdd_("A", +- (int[]){3}, (int[]){3}, +- M, (int[]){3}, +- S, +- Vt,(int[]){3}, +- U, (int[]){3}, +- work, +- (int[]){(int)lwork_query}, +- iwork, +- &info, +- 1); +- if(info != 0) +- { +- // secret value to indicate that this is a fatal error. Needed for the +- // Python layer +- R01[0] = 1.; +- return false; +- } +- +- // I look at the second-lowest singular value. One 0 singular value is OK +- // (the other two can uniquely define my 3D basis). But two isn't OK: the +- // basis is no longer unique +- if(S[1] < 1e-12) +- { +- // Poorly-defined problem +- // +- // secret value to indicate that this is a potentially non-fatal error. +- // Needed for the Python layer +- R01[0] = 0.; +- return false; +- } +- +- memset(R01, 0, 9*sizeof(R01[0])); +- for(int i=0; i<3; i++) +- for(int j=0; j<3; j++) +- for(int k=0; k<3; k++) +- // inner( U[i,:], V[j,:] +- R01[i*3 + j] += U[i*3 + k]*Vt[j + k*3]; +- +- // det(R01) is now +1 or -1. If it's -1, then this contains a mirror, and thus +- // is not a physical rotation. I compensate by negating the least-important +- // pair of singular vectors +- const double det_R = +- R01[0]*(R01[4]*R01[8]-R01[5]*R01[7]) - +- R01[1]*(R01[3]*R01[8]-R01[5]*R01[6]) + +- R01[2]*(R01[3]*R01[7]-R01[4]*R01[6]); +- if(det_R < 0) +- { +- memset(R01, 0, 9*sizeof(R01[0])); +- +- for(int i=0; i<3; i++) +- for(int j=0; j<3; j++) +- { +- int k; +- for(k=0; k<2; k++) +- R01[i*3 + j] += U[i*3 + k]*Vt[j + k*3]; +- R01[i*3 + j] -= U[i*3 + k]*Vt[j + k*3]; +- } +- } +- +- return true; +-} +- +-bool mrcal_align_procrustes_vectors_R01(// out +- double* R01, +- // in +- const int N, +- // (N,3) arrays +- const double* v0, +- const double* v1, +- +- // (N,) array; may be NULL to use an even +- // weighting +- const double* weights) +-{ +- return _align_procrustes_vectors_R01(R01,N,v0,v1,NULL,NULL,weights); +-} +- +-bool mrcal_align_procrustes_points_Rt01(// out +- double* Rt01, +- // in +- const int N, +- // (N,3) arrays +- const double* p0, +- const double* p1, +- +- // (N,) array; may be NULL to use an even +- // weighting +- const double* weights) +-{ +- double pmean0[3] = {}; +- double pmean1[3] = {}; +- +- for(int i=0; i +Date: Wed, 4 Dec 2024 00:58:21 -0500 +Subject: [PATCH 7/8] Place all C headers in an extern "C" block + +--- + mrcal.cpp | 5 ++++- + 1 file changed, 4 insertions(+), 1 deletion(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index 1560cd106ff5bcfe49476f785aa8108119318b3a..d4a2415a5730a48671b2716960bc343cdbd01295 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -14,19 +14,22 @@ + #include + #include + ++extern "C" { + #include ++} + #include + #include + #include + #include + ++extern "C" { + #include "mrcal.h" + #include "minimath/minimath.h" + #include "cahvore.h" + #include "minimath/minimath-extra.h" + #include "util.h" + #include "scales.h" +- ++} + #define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0) + + #define restrict __restrict diff --git a/upstream_utils/mrcal_patches/0008-Fix-UB-in-mrcal.patch b/upstream_utils/mrcal_patches/0008-Fix-UB-in-mrcal.patch new file mode 100644 index 0000000000..0014ab88cf --- /dev/null +++ b/upstream_utils/mrcal_patches/0008-Fix-UB-in-mrcal.patch @@ -0,0 +1,70 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Matt +Date: Thu, 5 Dec 2024 00:10:15 -0500 +Subject: [PATCH 8/8] Fix UB in mrcal + +--- + mrcal.cpp | 27 ++++++++++++++++----------- + 1 file changed, 16 insertions(+), 11 deletions(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index d4a2415a5730a48671b2716960bc343cdbd01295..0b7263f58164bfe9f42447741a08366262195f7c 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -4582,7 +4582,6 @@ void optimizer_callback(// input state + // Construct the FULL intrinsics vector, based on either the + // optimization vector or the inputs, depending on what we're optimizing + double* intrinsics_here = &intrinsics_all[icam_intrinsics][0]; +- double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore]; + + int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, +@@ -4604,15 +4603,20 @@ void optimizer_callback(// input state + &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics], + Ncore*sizeof(double) ); + } +- if( ctx->problem_selections.do_optimize_intrinsics_distortions ) +- { +- for(int i = 0; iNintrinsics-Ncore; i++) +- distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION; ++ int nDistortion = ctx->Nintrinsics-Ncore; ++ if (nDistortion) { ++ double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore]; ++ if( ctx->problem_selections.do_optimize_intrinsics_distortions ) ++ { ++ for(int i = 0; iNintrinsics-Ncore; i++) ++ distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION; ++ } ++ else { ++ memcpy( distortions_here, ++ &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore], ++ (ctx->Nintrinsics-Ncore)*sizeof(double) ); ++ } + } +- else +- memcpy( distortions_here, +- &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore], +- (ctx->Nintrinsics-Ncore)*sizeof(double) ); + } + for(int icam_extrinsics=0; + icam_extrinsicsNcameras_extrinsics; +@@ -4699,6 +4703,7 @@ void optimizer_callback(// input state + + int splined_intrinsics_grad_irun = 0; + ++ bool camera_at_identity = icam_extrinsics < 0; + project(q_hypothesis.data(), + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +@@ -4720,9 +4725,9 @@ void optimizer_callback(// input state + + // input + intrinsics_all[icam_intrinsics].data(), +- &camera_rt[icam_extrinsics], &frame_rt, ++ camera_at_identity ? NULL : &camera_rt[icam_extrinsics], &frame_rt, + ctx->calobject_warp == NULL ? NULL : &calobject_warp_local, +- icam_extrinsics < 0, ++ camera_at_identity, + &ctx->lensmodel, &ctx->precomputed, + ctx->calibration_object_spacing, + ctx->calibration_object_width_n, diff --git a/wpical/.styleguide b/wpical/.styleguide new file mode 100644 index 0000000000..afaf7a1075 --- /dev/null +++ b/wpical/.styleguide @@ -0,0 +1,36 @@ +cppHeaderFileInclude { + \.h$ + \.inc$ + \.inl$ +} + +cppSrcFileInclude { + \.cpp$ +} + +generatedFileExclude { + src/main/native/resources/ + src/main/native/thirdparty/ + src/main/native/win/wpical.ico + src/main/native/mac/wpical.icns +} + +repoRootNameOverride { + wpical +} + +includeOtherLibs { + ^GLFW + ^ceres/ + ^fmt/ + ^frc/ + ^gtest/ + ^imgui + ^implot\.h$ + ^mrcal_wrapper\.h$ + ^opencv2\.h$ + ^portable-file-dialogs\.h$ + ^tagpose\.h$ + ^wpi/ + ^wpigui +} diff --git a/wpical/CMakeLists.txt b/wpical/CMakeLists.txt new file mode 100644 index 0000000000..f07d52755a --- /dev/null +++ b/wpical/CMakeLists.txt @@ -0,0 +1,129 @@ +project(wpical LANGUAGES C CXX Fortran) + +include(CompileWarnings) +include(GenResources) +include(LinkMacOSGUI) +include(AddTest) + +configure_file(src/main/generate/WPILibVersion.cpp.in WPILibVersion.cpp) +generate_resources(src/main/native/resources generated/main/cpp WPIcal wpical wpical_resources_src) + +file(GLOB wpical_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp) +file( + GLOB_RECURSE wpical_thirdparty_src + src/main/native/thirdparty/libdogleg/src/*.cpp + src/main/native/thirdparty/mrcal/src/*.c + src/main/native/thirdparty/mrcal/src/*.cpp + src/main/native/thirdparty/mrcal_java/src/*.cpp +) + +if(WIN32) + set(wpical_rc src/main/native/win/wpical.rc) +elseif(APPLE) + set(MACOSX_BUNDLE_ICON_FILE wpical.icns) + set(APP_ICON_MACOSX src/main/native/mac/wpical.icns) + set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources") +endif() + +add_executable( + wpical + ${wpical_src} + ${wpical_thirdparty_src} + ${wpical_resources_src} + ${wpical_rc} + ${APP_ICON_MACOSX} +) +wpilib_link_macos_gui(wpical) +wpilib_target_warnings(wpical) +target_include_directories( + wpical + PRIVATE + src/main/native/include + src/main/native/thirdparty/libdogleg/include + src/main/native/thirdparty/mrcal/include + src/main/native/thirdparty/mrcal_java/include +) + +if(MSVC) + set(compile_flags + /wd4047 + /wd4098 + /wd4267 + /wd4068 + /wd4101 + /wd4200 + /wd4576 + /wd4715 + ) +elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(compile_flags + -Wno-format-nonliteral + -Wno-unused-variable + -Wno-missing-field-initializers + -Wno-gnu-anonymous-struct + -Wno-c99-extensions + -Wno-zero-length-array + -Wno-nested-anon-types + -Wno-sign-compare + -Wno-unused-function + -Wno-missing-braces + -Wno-null-conversion + -Wno-unused-but-set-variable + ) +else() + set(compile_flags + -Wno-format-nonliteral + -Wno-unused-variable + -Wno-unused-function + -Wno-sign-compare + -Wno-missing-field-initializers + ) +endif() +target_compile_options(wpical PRIVATE ${compile_flags}) + +find_package(OpenCV REQUIRED) +find_package(Ceres CONFIG REQUIRED) +target_link_libraries( + wpical + apriltag + ${OpenCV_LIBS} + wpigui + wpiutil + Ceres::ceres +) + +if(WIN32) + set_target_properties(wpical PROPERTIES WIN32_EXECUTABLE YES) +elseif(APPLE) + set_target_properties(wpical PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "wpical") +endif() + +if(WITH_TESTS) + wpilib_add_test(wpical src/test/native/cpp) + wpilib_link_macos_gui(wpical_test) + target_sources(wpical_test PRIVATE ${wpical_src} ${wpical_thirdparty_src}) + target_compile_definitions(wpical_test PRIVATE RUNNING_WPICAL_TESTS) + + if(MSVC) + target_compile_options(wpical_test PRIVATE /utf-8) + endif() + + target_compile_options(wpical_test PRIVATE ${compile_flags}) + target_include_directories( + wpical_test + PRIVATE + src/main/native/include + src/main/native/thirdparty/libdogleg/include + src/main/native/thirdparty/mrcal/include + src/main/native/thirdparty/mrcal_java/include + ) + target_link_libraries( + wpical_test + googletest + apriltag + ${OpenCV_LIBS} + wpigui + wpiutil + Ceres::ceres + ) +endif() diff --git a/wpical/Info.plist b/wpical/Info.plist new file mode 100644 index 0000000000..473286a8aa --- /dev/null +++ b/wpical/Info.plist @@ -0,0 +1,32 @@ + + + + + CFBundleName + WPIcal + CFBundleExecutable + wpical + CFBundleDisplayName + WPIcal + CFBundleIdentifier + edu.wpi.first.tools.WPIcal + CFBundleIconFile + ov.icns + CFBundlePackageType + APPL + CFBundleSupportedPlatforms + + MacOSX + + CFBundleInfoDictionaryVersion + 6.0 + CFBundleShortVersionString + 2021 + CFBundleVersion + 2021 + LSMinimumSystemVersion + 10.11 + NSHighResolutionCapable + + + diff --git a/wpical/build.gradle b/wpical/build.gradle new file mode 100644 index 0000000000..eea250c903 --- /dev/null +++ b/wpical/build.gradle @@ -0,0 +1,249 @@ +import org.gradle.internal.os.OperatingSystem + +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64')) { + return; +} + +description = "Field Calibration Tool" + +apply plugin: 'c' +apply plugin: 'cpp' +apply plugin: 'visual-studio' +apply plugin: 'google-test-test-suite' +apply plugin: 'edu.wpi.first.NativeUtils' + +if (OperatingSystem.current().isWindows()) { + apply plugin: 'windows-resources' +} + +ext { + nativeName = 'wpical' + useCpp = true + sharedCvConfigs = [ + wpicalTest: []] + staticCvConfigs = [] +} + +apply from: "${rootDir}/shared/resources.gradle" +apply from: "${rootDir}/shared/config.gradle" + +def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in") +def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp") + +apply from: "${rootDir}/shared/ceres.gradle" +apply from: "${rootDir}/shared/opencv.gradle" + +task generateCppVersion() { + description = 'Generates the wpilib version class' + group = 'WPILib' + + outputs.file wpilibVersionFileOutput + inputs.file wpilibVersionFileInput + + if (wpilibVersioning.releaseMode) { + outputs.upToDateWhen { false } + } + + // We follow a simple set of checks to determine whether we should generate a new version file: + // 1. If the release type is not development, we generate a new version file + // 2. If there is no generated version number, we generate a new version file + // 3. If there is a generated build number, and the release type is development, then we will + // only generate if the publish task is run. + doLast { + def version = wpilibVersioning.version.get() + println "Writing version ${version} to $wpilibVersionFileOutput" + + if (wpilibVersionFileOutput.exists()) { + wpilibVersionFileOutput.delete() + } + def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version) + wpilibVersionFileOutput.write(read) + } +} + +gradle.taskGraph.addTaskExecutionGraphListener { graph -> + def willPublish = graph.hasTask(publish) + if (willPublish) { + generateCppVersion.outputs.upToDateWhen { false } + } +} + +def generateTask = createGenerateResourcesTask('main', 'WPIcal', 'wpical', project) + +project(':').libraryBuild.dependsOn build +tasks.withType(CppCompile) { + dependsOn generateTask + dependsOn generateCppVersion +} + +// Suppress warnings +nativeUtils.platformConfigs.each { + if (it.name.contains('windows')) { + it.cCompiler.args.add("/wd4047") + it.cCompiler.args.add("/wd4098") + it.cCompiler.args.add("/wd4267") + it.cppCompiler.args.add("/wd4068") + it.cppCompiler.args.add("/wd4101") + it.cppCompiler.args.add("/wd4200") + it.cppCompiler.args.add("/wd4576") + it.cppCompiler.args.add("/wd4715") + } else if (it.name.contains('osx')) { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cCompiler.args.remove("-pedantic") + it.cCompiler.args.add("-Wno-unused-variable") + it.cCompiler.args.add("-Wno-unused-function") + it.cCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.add("-Wno-missing-field-initializers") + it.cppCompiler.args.remove("-pedantic") + it.cppCompiler.args.add("-Wno-unused-variable") + it.cppCompiler.args.add("-Wno-unused-function") + it.cppCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.remove("-permissive") + it.cppCompiler.args.add("-fpermissive") + it.cppCompiler.args.add("-Wno-missing-braces") + it.cppCompiler.args.add("-Wno-null-conversion") + it.cppCompiler.args.add("-Wno-unused-but-set-variable") + } else { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cCompiler.args.remove("-pedantic") + it.cCompiler.args.add("-Wno-unused-variable") + it.cCompiler.args.add("-Wno-unused-function") + it.cCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.add("-Wno-missing-field-initializers") + it.cppCompiler.args.remove("-pedantic") + it.cppCompiler.args.add("-Wno-unused-variable") + it.cppCompiler.args.add("-Wno-unused-function") + it.cppCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.add("-Wno-deprecated-declarations") + it.cppCompiler.args.add("-Wno-deprecated-enum-enum-conversion") + it.cppCompiler.args.remove("-permissive") + it.cppCompiler.args.add("-fpermissive") + } +} +def testResources = "\"$rootDir/wpical/src/main/native/resources\"".replace("\\", "/") +model { + components { + "${nativeName}"(NativeExecutableSpec) { + baseName = 'wpical' + sources { + cpp { + source { + srcDirs 'src/main/native/cpp', 'src/main/native/thirdparty/libdogleg/src', 'src/main/native/thirdparty/mrcal/src', 'src/main/native/thirdparty/mrcal_java/src', "$buildDir/generated/main/cpp" + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/include', 'src/main/native/thirdparty/libdogleg/include', 'src/main/native/thirdparty/mrcal/generated', 'src/main/native/thirdparty/mrcal/include', 'src/main/native/thirdparty/mrcal/include/minimath', 'src/main/native/thirdparty/mrcal_java/include' + } + } + c { + source { + srcDirs 'src/main/native/thirdparty/mrcal/src' + include '**/*.c' + } + exportedHeaders { + srcDirs 'src/main/native/include', 'src/main/native/thirdparty/libdogleg/include', 'src/main/native/thirdparty/mrcal/generated', 'src/main/native/thirdparty/mrcal/include', 'src/main/native/thirdparty/mrcal/include/minimath' + } + } + if (OperatingSystem.current().isWindows()) { + rc.source { + srcDirs 'src/main/native/win' + include '*.rc' + } + } + } + binaries.all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name.startsWith("linuxarm")) { + it.buildable = false + return + } + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' + lib project: ':wpigui', library: 'wpigui', linkage: 'static' + lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static' + lib project: ':apriltag', library: 'apriltag', linkage: 'static' + nativeUtils.useRequiredLibrary(it, 'ceres') + nativeUtils.useRequiredLibrary(it, 'opencv_static') + it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT' + if (it.targetPlatform.operatingSystem.isWindows()) { + it.cppCompiler.define('GLOG_DEPRECATED', '__declspec(deprecated)') + } else { + it.cppCompiler.define('GLOG_DEPRECATED', '[[deprecated]]') + } + if (it.targetPlatform.operatingSystem.isWindows()) { + // Comdlg32.lib is needed for GetSaveFileNameA + // dbghelp.lib is needed for glog functions + it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' << 'Comdlg32.lib' << 'dbghelp.lib' + } else if (it.targetPlatform.operatingSystem.isMacOsX()) { + it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' << '-framework' << 'Accelerate' + } else { + it.linker.args << '-lX11' << "-lgfortran" + if (it.targetPlatform.name.startsWith('linuxarm')) { + it.linker.args << '-lGL' + } + } + } + } + } + testSuites { + "${nativeName}Test"(GoogleTestTestSuiteSpec) { + for(NativeComponentSpec c : $.components) { + if (c.name == nativeName) { + testing c + break + } + } + sources { + cpp { + source { + srcDirs 'src/test/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/test/native/include', 'src/main/native/cpp' + } + } + } + binaries.all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name.startsWith("linuxarm")) { + it.buildable = false + return + } + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' + lib project: ':wpigui', library: 'wpigui', linkage: 'static' + lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static' + lib project: ':apriltag', library: 'apriltag', linkage: 'static' + nativeUtils.useRequiredLibrary(it, 'ceres') + nativeUtils.useRequiredLibrary(it, 'opencv_static') + it.cppCompiler.define('PROJECT_ROOT_PATH', testResources) + it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT' + if (it.targetPlatform.name == nativeUtils.wpi.platforms.windowsarm64) { + // https://developercommunity.visualstudio.com/t/-imp-std-init-once-complete-unresolved-external-sy/1684365#T-N10041864 + it.linker.args << '/alternatename:__imp___std_init_once_complete=__imp_InitOnceComplete' + it.linker.args << '/alternatename:__imp___std_init_once_begin_initialize=__imp_InitOnceBeginInitialize' + } + if (it.targetPlatform.operatingSystem.isWindows()) { + it.cppCompiler.define('GLOG_DEPRECATED', '__declspec(deprecated)') + } else { + it.cppCompiler.define('GLOG_DEPRECATED', '[[deprecated]]') + } + lib project: ':thirdparty:googletest', library: 'googletest', linkage: 'static' + it.cppCompiler.define("RUNNING_WPICAL_TESTS") + if (it.targetPlatform.operatingSystem.isWindows()) { + it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' + // Comdlg32.lib is needed for GetSaveFileNameA + // dbghelp.lib is needed for glog functions + // advapi32.lib is needed for __imp_RegDeleteKeyA and other __imp_Reg functions for icvLoadWindowPos and the save variant + it.linker.args << 'advapi32.lib' << 'Comdlg32.lib' << 'dbghelp.lib' + } else if (it.targetPlatform.operatingSystem.isMacOsX()) { + it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' << '-framework' << 'Accelerate' + } else { + it.linker.args << '-lX11' << '-lgfortran' + if (it.targetPlatform.name.startsWith('linuxarm')) { + it.linker.args << '-lGL' + } + } + } + } + } +} + +apply from: 'publish.gradle' diff --git a/wpical/generate_mrcal.py b/wpical/generate_mrcal.py new file mode 100755 index 0000000000..eacfabfda0 --- /dev/null +++ b/wpical/generate_mrcal.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +import argparse +import subprocess +import sys +from pathlib import Path + + +def main(argv): + script_path = Path(__file__).resolve() + dirname = script_path.parent + + parser = argparse.ArgumentParser() + parser.add_argument( + "--output_directory", + help="Optional. If set, will output the generated files to this directory, otherwise it will use a path relative to the script", + default=dirname / "src/main/native/thirdparty/mrcal/generated/", + type=Path, + ) + args = parser.parse_args(argv) + + args.output_directory.mkdir(parents=True, exist_ok=True) + result = subprocess.run( + f"{dirname}/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl", + capture_output=True, + ) + (args.output_directory / "minimath_generated.h").write_text( + str(result.stdout, encoding="UTF8") + ) + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/wpical/publish.gradle b/wpical/publish.gradle new file mode 100644 index 0000000000..d00f47aafb --- /dev/null +++ b/wpical/publish.gradle @@ -0,0 +1,124 @@ +apply plugin: 'maven-publish' + +def baseArtifactId = 'wpical' +def artifactGroupId = 'edu.wpi.first.tools' +def zipBaseName = '_GROUP_edu_wpi_first_tools_ID_wpical_CLS' + +def outputsFolder = file("$project.buildDir/outputs") + +model { + tasks { + // Create the run task. + $.components.wpical.binaries.each { bin -> + if (bin.buildable && bin.name.toLowerCase().contains("debug") && nativeUtils.isNativeDesktopPlatform(bin.targetPlatform)) { + Task run = project.tasks.create("run", Exec) { + commandLine bin.tasks.install.runScriptFile.get().asFile.toString() + } + run.dependsOn bin.tasks.install + } + } + } + publishing { + def wpicalTaskList = [] + $.components.each { component -> + component.binaries.each { binary -> + if (binary in NativeExecutableBinarySpec && binary.component.name.contains("wpical")) { + if (binary.buildable && (binary.name.contains('Release') || binary.name.contains('release'))) { + // We are now in the binary that we want. + // This is the default application path for the ZIP task. + def applicationPath = binary.executable.file + def icon = file("$project.projectDir/src/main/native/mac/wpical.icns") + + // Create the ZIP. + def task = project.tasks.create("copywpicalExecutable" + binary.targetPlatform.operatingSystem.name + binary.targetPlatform.architecture.name, Zip) { + description("Copies the wpical executable to the outputs directory.") + destinationDirectory = outputsFolder + + archiveBaseName = zipBaseName + duplicatesStrategy = 'exclude' + archiveClassifier = nativeUtils.getPublishClassifier(binary) + + from(licenseFile) { + into '/' + } + + if (binary.targetPlatform.operatingSystem.isWindows()) { + def exePath = binary.executable.file.absolutePath + exePath = exePath.substring(0, exePath.length() - 4) + def pdbPath = new File(exePath + '.pdb') + from(pdbPath) + } + + into(nativeUtils.getPlatformPath(binary)) + } + + if (binary.targetPlatform.operatingSystem.isMacOsX()) { + // Create the macOS bundle. + def bundleTask = project.tasks.create("bundlewpicalOsxApp" + binary.targetPlatform.architecture.name, Copy) { + description("Creates a macOS application bundle for wpical") + from(file("$project.projectDir/Info.plist")) + into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/wpical.app/Contents")) + into("MacOS") { + with copySpec { + from binary.executable.file + } + } + into("Resources") { + with copySpec { + from icon + } + } + + inputs.property "HasDeveloperId", project.hasProperty("developerID") + + doLast { + if (project.hasProperty("developerID")) { + // Get path to binary. + exec { + workingDir rootDir + def args = [ + "sh", + "-c", + "codesign --force --strict --deep " + + "--timestamp --options=runtime " + + "--verbose -s ${project.findProperty("developerID")} " + + "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/wpical.app/" + ] + commandLine args + } + } + } + } + + // Reset the application path if we are creating a bundle. + applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name") + task.from(applicationPath) + project.build.dependsOn bundleTask + + bundleTask.dependsOn binary.tasks.link + task.dependsOn(bundleTask) + } else { + task.from(applicationPath) + } + + task.dependsOn binary.tasks.link + wpicalTaskList.add(task) + project.build.dependsOn task + project.artifacts { task } + addTaskToCopyAllOutputs(task) + } + } + } + } + + publications { + wpical(MavenPublication) { + wpicalTaskList.each { artifact it } + + artifactId = baseArtifactId + groupId = artifactGroupId + version wpilibVersioning.version.get() + } + } + } +} diff --git a/wpical/src/main/generate/WPILibVersion.cpp.in b/wpical/src/main/generate/WPILibVersion.cpp.in new file mode 100644 index 0000000000..cfe2441158 --- /dev/null +++ b/wpical/src/main/generate/WPILibVersion.cpp.in @@ -0,0 +1,7 @@ +/** + * Autogenerated file! Do not manually edit this file. This version is regenerated + * any time the publish task is run, or when this file is deleted. + */ +const char* GetWPILibVersion() { + return "${wpilib_version}"; +} diff --git a/wpical/src/main/native/cpp/WPIcal.cpp b/wpical/src/main/native/cpp/WPIcal.cpp new file mode 100644 index 0000000000..75066b5124 --- /dev/null +++ b/wpical/src/main/native/cpp/WPIcal.cpp @@ -0,0 +1,591 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gui = wpi::gui; + +const char* GetWPILibVersion(); + +#ifdef __linux__ +const bool showDebug = false; +#else +const bool showDebug = true; +#endif + +namespace wpical { +std::string_view GetResource_wpical_16_png(); +std::string_view GetResource_wpical_32_png(); +std::string_view GetResource_wpical_48_png(); +std::string_view GetResource_wpical_64_png(); +std::string_view GetResource_wpical_128_png(); +std::string_view GetResource_wpical_256_png(); +std::string_view GetResource_wpical_512_png(); +} // namespace wpical + +void drawCheck() { + ImGui::SameLine(); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + + ImVec2 pos = ImGui::GetCursorScreenPos(); + float size = ImGui::GetFontSize(); + + ImVec2 p1 = ImVec2(pos.x + size * 0.25f, pos.y + size * 0.5f); + ImVec2 p2 = ImVec2(pos.x + size * 0.45f, pos.y + size * 0.7f); + ImVec2 p3 = ImVec2(pos.x + size * 0.75f, pos.y + size * 0.3f); + + float thickness = 3.0f; + draw_list->AddLine(p1, p2, IM_COL32(0, 255, 0, 255), thickness); + draw_list->AddLine(p2, p3, IM_COL32(0, 255, 0, 255), thickness); + ImGui::NewLine(); +} + +static void DisplayGui() { + ImGui::GetStyle().WindowRounding = 0; + + // fill entire OS window with this window + ImGui::SetNextWindowPos(ImVec2(0, 0)); + int width, height; + glfwGetWindowSize(gui::GetSystemWindow(), &width, &height); + ImGui::SetNextWindowSize( + ImVec2(static_cast(width), static_cast(height))); + + ImGui::Begin("Entries", nullptr, + ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_MenuBar | + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | + ImGuiWindowFlags_NoCollapse); + + // main menu + ImGui::BeginMenuBar(); + gui::EmitViewMenu(); + if (ImGui::BeginMenu("View")) { + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + + static std::unique_ptr camera_intrinsics_selector; + static std::string selected_camera_intrinsics; + + static std::unique_ptr field_map_selector; + static std::string selected_field_map; + + static std::unique_ptr + field_calibration_directory_selector; + static std::string selected_field_calibration_directory; + + static std::unique_ptr download_directory_selector; + static std::string selected_download_directory; + + static std::string calibration_json_path; + + cameracalibration::CameraModel cameraModel = { + .intrinsic_matrix = Eigen::Matrix::Identity(), + .distortion_coefficients = Eigen::Matrix::Zero(), + .avg_reprojection_error = 0.0}; + static bool mrcal = true; + + static double squareWidth = 0.709; + static double markerWidth = 0.551; + static int boardWidth = 12; + static int boardHeight = 8; + static double imagerWidth = 1920; + static double imagerHeight = 1080; + + static int pinnedTag = 1; + + static int focusedTag = 1; + static int referenceTag = 1; + + static Fieldmap currentCalibrationMap; + static Fieldmap currentReferenceMap; + + // camera matrix selector button + if (ImGui::Button("Upload Camera Intrinsics")) { + camera_intrinsics_selector = std::make_unique( + "Select Camera Intrinsics JSON", "", + std::vector{"JSON", "*.json"}, pfd::opt::none); + } + + ImGui::SameLine(); + ImGui::Text("Or"); + ImGui::SameLine(); + + // camera calibration button + if (ImGui::Button("Calibrate Camera")) { + selected_camera_intrinsics.clear(); + ImGui::OpenPopup("Camera Calibration"); + } + + if (camera_intrinsics_selector) { + auto selectedFiles = camera_intrinsics_selector->result(); + if (!selectedFiles.empty()) { + selected_camera_intrinsics = selectedFiles[0]; + } + camera_intrinsics_selector.reset(); + } + + if (!selected_camera_intrinsics.empty()) { + drawCheck(); + } + + // field json selector button + if (ImGui::Button("Select Field Map JSON")) { + field_map_selector = std::make_unique( + "Select Json File", "", + std::vector{"JSON Files", "*.json"}, pfd::opt::none); + } + + if (field_map_selector) { + auto selectedFiles = field_map_selector->result(); + if (!selectedFiles.empty()) { + selected_field_map = selectedFiles[0]; + } + field_map_selector.reset(); + } + + if (!selected_field_map.empty()) { + drawCheck(); + } + + // field calibration directory selector button + if (ImGui::Button("Select Field Calibration Folder")) { + field_calibration_directory_selector = std::make_unique( + "Select Field Calibration Folder", ""); + } + + if (field_calibration_directory_selector) { + auto selectedFiles = field_calibration_directory_selector->result(); + if (!selectedFiles.empty()) { + selected_field_calibration_directory = selectedFiles; + } + field_calibration_directory_selector.reset(); + } + + if (!selected_field_calibration_directory.empty()) { + drawCheck(); + } + + // pinned tag text field + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Pinned Tag", &pinnedTag); + + if (pinnedTag < 1) { + pinnedTag = 1; + } else if (pinnedTag > 16) { + pinnedTag = 16; + } + + // calibrate button + if (ImGui::Button("Calibrate!!!")) { + if (!selected_field_calibration_directory.empty() && + !selected_camera_intrinsics.empty() && !selected_field_map.empty() && + pinnedTag > 0 && pinnedTag <= 16) { + download_directory_selector = + std::make_unique("Select Download Folder", ""); + if (download_directory_selector) { + auto selectedFiles = download_directory_selector->result(); + if (!selectedFiles.empty()) { + selected_download_directory = selectedFiles; + } + download_directory_selector.reset(); + } + + calibration_json_path = selected_download_directory + "/output.json"; + + int calibrationOutput = fieldcalibration::calibrate( + selected_field_calibration_directory.c_str(), calibration_json_path, + selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag, + showDebug); + + if (calibrationOutput == 1) { + ImGui::OpenPopup("Fmap Conversion failed"); + } else if (calibrationOutput == 0) { + std::ifstream caljsonpath(calibration_json_path); + try { + wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath)); + std::ofstream out(selected_download_directory + "/output.fmap"); + out << fmap.dump(4); + out.close(); + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Visualize Calibration"); + } catch (...) { + ImGui::OpenPopup("Field Calibration Error"); + } + } + } + } + if (ImGui::Button("Visualize")) { + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Visualize Calibration"); + } + if (selected_field_calibration_directory.empty() || + selected_camera_intrinsics.empty() || selected_field_map.empty()) { + ImGui::TextWrapped( + "Some inputs are empty! please enter your camera calibration video, " + "field map, and field calibration directory"); + } else if (!(pinnedTag > 0 && pinnedTag <= 16)) { + ImGui::TextWrapped("Make sure the pinned tag is a valid april tag (1-16)"); + } else { + ImGui::TextWrapped("Calibration Ready"); + } + + // error popup window + if (ImGui::BeginPopupModal("Field Calibration Error", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped( + "Field Calibration Failed - please try again, ensuring that:"); + ImGui::TextWrapped( + "- You have selected the correct camera intrinsics JSON or camera " + "calibration video"); + ImGui::TextWrapped("- You have selected the correct ideal field map JSON"); + ImGui::TextWrapped( + "- You have selected the correct field calibration video directory"); + ImGui::TextWrapped( + "- Your field calibration video directory contains only field " + "calibration videos and no other files"); + ImGui::TextWrapped("- Your pinned tag is a valid FRC Apriltag"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + if (ImGui::BeginPopupModal("Fmap Conversion failed", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped( + "Fmap conversion failed - you can still use the calibration output on " + "Limelight platforms by converting the .json output to .fmap using the " + "Limelight map builder tool"); + ImGui::TextWrapped("https://tools.limelightvision.io/map-builder"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + // successful field calibration popup window + if (ImGui::BeginPopupModal("Success", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped("Success, output JSON generated in selected directory"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + // camera calibration popup window + if (ImGui::BeginPopupModal("Camera Calibration", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + // Camera Calibration Error calibration popup window + if (ImGui::BeginPopupModal("Camera Calibration Error", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped( + "Camera calibration failed. Please make sure you have uploaded the " + "correct video file"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + if (ImGui::Button("MRcal")) { + mrcal = true; + } + + ImGui::SameLine(); + ImGui::Text("or"); + ImGui::SameLine(); + + if (ImGui::Button("OpenCV")) { + mrcal = false; + } + + if (mrcal) { + if (ImGui::Button("Select Camera Calibration Video")) { + camera_intrinsics_selector = std::make_unique( + "Select Camera Calibration Video", "", + std::vector{"Video Files", + "*.mp4 *.mov *.m4v *.mkv *.avi"}, + pfd::opt::none); + } + + if (camera_intrinsics_selector) { + auto selectedFiles = camera_intrinsics_selector->result(); + if (!selectedFiles.empty()) { + selected_camera_intrinsics = selectedFiles[0]; + } + camera_intrinsics_selector.reset(); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Square Width (in)", &squareWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Marker Width (in)", &markerWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Width (squares)", &boardWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Height (squares)", &boardHeight); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Image Width (pixels)", &imagerWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Image Height (pixels)", &imagerHeight); + + ImGui::Separator(); + if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) { + std::cout << "calibration button pressed" << std::endl; + int ret = cameracalibration::calibrate( + selected_camera_intrinsics.c_str(), cameraModel, markerWidth, + boardWidth, boardHeight, imagerWidth, imagerHeight, showDebug); + if (ret == 0) { + size_t lastSeparatorPos = + selected_camera_intrinsics.find_last_of("/\\"); + std::string output_file_path; + + if (lastSeparatorPos != std::string::npos) { + output_file_path = + selected_camera_intrinsics.substr(0, lastSeparatorPos) + .append("/cameracalibration.json"); + } + + selected_camera_intrinsics = output_file_path; + + cameracalibration::dumpJson(cameraModel, output_file_path); + ImGui::CloseCurrentPopup(); + } else if (ret == 1) { + std::cout << "calibration failed and popup ready" << std::endl; + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Camera Calibration Error"); + } else { + ImGui::CloseCurrentPopup(); + } + } + } else { + if (ImGui::Button("Select Camera Calibration Video")) { + camera_intrinsics_selector = std::make_unique( + "Select Camera Calibration Video", "", + std::vector{"Video Files", + "*.mp4 *.mov *.m4v *.mkv *.avi"}, + pfd::opt::none); + } + + if (camera_intrinsics_selector) { + auto selectedFiles = camera_intrinsics_selector->result(); + if (!selectedFiles.empty()) { + selected_camera_intrinsics = selectedFiles[0]; + } + camera_intrinsics_selector.reset(); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Square Width (in)", &squareWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Marker Width (in)", &markerWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Width (squares)", &boardWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Height (squares)", &boardHeight); + + ImGui::Separator(); + if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) { + std::cout << "calibration button pressed" << std::endl; + int ret = cameracalibration::calibrate( + selected_camera_intrinsics.c_str(), cameraModel, squareWidth, + markerWidth, boardWidth, boardHeight, showDebug); + if (ret == 0) { + size_t lastSeparatorPos = + selected_camera_intrinsics.find_last_of("/\\"); + std::string output_file_path; + + if (lastSeparatorPos != std::string::npos) { + output_file_path = + selected_camera_intrinsics.substr(0, lastSeparatorPos) + .append("/cameracalibration.json"); + } + + selected_camera_intrinsics = output_file_path; + + cameracalibration::dumpJson(cameraModel, output_file_path); + ImGui::CloseCurrentPopup(); + } else if (ret == 1) { + std::cout << "calibration failed and popup ready" << std::endl; + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Camera Calibration Error"); + } else { + ImGui::CloseCurrentPopup(); + } + } + } + + ImGui::SameLine(); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } + + // visualize calibration popup + if (ImGui::BeginPopupModal("Visualize Calibration", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + if (ImGui::Button("Load Calibrated Field")) { + calibration_json_path = + std::make_unique( + "Select Json File", "", + std::vector{"JSON Files", "*.json"}, pfd::opt::none) + ->result()[0]; + } + + if (!calibration_json_path.empty()) { + ImGui::SameLine(); + drawCheck(); + } + + if (ImGui::Button("Load Reference Field")) { + selected_field_map = + std::make_unique( + "Select Json File", "", + std::vector{"JSON Files", "*.json"}, pfd::opt::none) + ->result()[0]; + } + + if (!selected_field_map.empty()) { + ImGui::SameLine(); + drawCheck(); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Focused Tag", &focusedTag); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Reference Tag", &referenceTag); + + if (focusedTag < 1) { + focusedTag = 1; + } else if (focusedTag > 16) { + focusedTag = 16; + } + + if (referenceTag < 1) { + referenceTag = 1; + } else if (referenceTag > 16) { + referenceTag = 16; + } + + if (!calibration_json_path.empty() && !selected_field_map.empty()) { + std::ifstream calJson(calibration_json_path); + std::ifstream refJson(selected_field_map); + + currentCalibrationMap = Fieldmap(wpi::json::parse(calJson)); + currentReferenceMap = Fieldmap(wpi::json::parse(refJson)); + + double xDiff = currentReferenceMap.getTag(focusedTag).xPos - + currentCalibrationMap.getTag(focusedTag).xPos; + double yDiff = currentReferenceMap.getTag(focusedTag).yPos - + currentCalibrationMap.getTag(focusedTag).yPos; + double zDiff = currentReferenceMap.getTag(focusedTag).zPos - + currentCalibrationMap.getTag(focusedTag).zPos; + double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot - + currentCalibrationMap.getTag(focusedTag).yawRot; + double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot - + currentCalibrationMap.getTag(focusedTag).pitchRot; + double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot - + currentCalibrationMap.getTag(focusedTag).rollRot; + + double xRef = currentCalibrationMap.getTag(referenceTag).xPos - + currentCalibrationMap.getTag(focusedTag).xPos; + double yRef = currentCalibrationMap.getTag(referenceTag).yPos - + currentCalibrationMap.getTag(focusedTag).yPos; + double zRef = currentCalibrationMap.getTag(referenceTag).zPos - + currentCalibrationMap.getTag(focusedTag).zPos; + + ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str()); + ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str()); + ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str()); + + ImGui::TextWrapped( + "Yaw Difference %s°", + std::to_string( + Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi))) + .c_str()); + ImGui::TextWrapped( + "Pitch Difference %s°", + std::to_string( + Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi))) + .c_str()); + ImGui::TextWrapped( + "Roll Difference %s°", + std::to_string( + Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi))) + .c_str()); + + ImGui::NewLine(); + + ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str()); + ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str()); + ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str()); + } + + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + ImGui::End(); +} + +#ifndef RUNNING_WPICAL_TESTS +#ifdef _WIN32 +int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine, + int nCmdShow) { + int argc = __argc; + char** argv = __argv; +#else +int main(int argc, char** argv) { +#endif + std::string_view saveDir; + if (argc == 2) { + saveDir = argv[1]; + } + + gui::CreateContext(); + + gui::AddIcon(wpical::GetResource_wpical_16_png()); + gui::AddIcon(wpical::GetResource_wpical_32_png()); + gui::AddIcon(wpical::GetResource_wpical_48_png()); + gui::AddIcon(wpical::GetResource_wpical_64_png()); + gui::AddIcon(wpical::GetResource_wpical_128_png()); + gui::AddIcon(wpical::GetResource_wpical_256_png()); + gui::AddIcon(wpical::GetResource_wpical_512_png()); + + gui::AddLateExecute(DisplayGui); + + gui::Initialize("wpical", 600, 400); + gui::Main(); + + gui::DestroyContext(); + + return 0; +} +#endif diff --git a/wpical/src/main/native/cpp/cameracalibration.cpp b/wpical/src/main/native/cpp/cameracalibration.cpp new file mode 100644 index 0000000000..270de38af9 --- /dev/null +++ b/wpical/src/main/native/cpp/cameracalibration.cpp @@ -0,0 +1,404 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "cameracalibration.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +static bool filter(std::vector charuco_corners, + std::vector charuco_ids, + std::vector> marker_corners, + std::vector marker_ids, int board_width, + int board_height) { + if (charuco_ids.empty() || charuco_corners.empty()) { + return false; + } + + if (charuco_corners.size() < 10 || charuco_ids.size() < 10) { + return false; + } + + for (int i = 0; i < charuco_ids.size(); i++) { + if (charuco_ids.at(i) > (board_width - 1) * (board_height - 1) - 1) { + return false; + } + } + + for (int i = 0; i < marker_ids.size(); i++) { + if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) { + return false; + } + } + + return true; +} + +int cameracalibration::calibrate(const std::string& input_video, + CameraModel& camera_model, float square_width, + float marker_width, int board_width, + int board_height, bool show_debug_window) { + // ChArUco Board + cv::aruco::Dictionary aruco_dict = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); + cv::Ptr charuco_board = new cv::aruco::CharucoBoard( + cv::Size(board_width, board_height), square_width * 0.0254, + marker_width * 0.0254, aruco_dict); + cv::aruco::CharucoDetector charuco_detector(*charuco_board); + + // Video capture + cv::VideoCapture video_capture(input_video); + cv::Size frame_shape; + + std::vector> all_charuco_corners; + std::vector> all_charuco_ids; + + std::vector> all_obj_points; + std::vector> all_img_points; + + while (video_capture.grab()) { + cv::Mat frame; + video_capture.retrieve(frame); + + cv::Mat debug_image = frame; + + if (frame.empty()) { + break; + } + + // Detect + cv::Mat frame_gray; + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + frame_shape = frame_gray.size(); + + std::vector charuco_corners; + std::vector charuco_ids; + std::vector> marker_corners; + std::vector marker_ids; + + std::vector obj_points; + std::vector img_points; + + charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, + marker_corners, marker_ids); + + if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, + board_width, board_height)) { + continue; + } + + charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, + img_points); + + all_charuco_corners.push_back(charuco_corners); + all_charuco_ids.push_back(charuco_ids); + + all_obj_points.push_back(obj_points); + all_img_points.push_back(img_points); + + if (show_debug_window) { + cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); + cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, + charuco_ids); + cv::imshow("Frame", debug_image); + if (cv::waitKey(1) == 'q') { + break; + } + } + } + + video_capture.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + // Calibrate + cv::Mat camera_matrix, dist_coeffs; + std::vector r_vecs, t_vecs; + std::vector std_dev_intrinsics, std_dev_extrinsics, per_view_errors; + double repError; + + try { + // see https://stackoverflow.com/a/75865177 + int flags = cv::CALIB_RATIONAL_MODEL | cv::CALIB_USE_LU; + repError = cv::calibrateCamera( + all_obj_points, all_img_points, frame_shape, camera_matrix, dist_coeffs, + r_vecs, t_vecs, cv::noArray(), cv::noArray(), cv::noArray(), flags); + } catch (...) { + std::cout << "calibration failed" << std::endl; + return 1; + } + + std::vector matrix = {camera_matrix.begin(), + camera_matrix.end()}; + + std::vector distortion = {dist_coeffs.begin(), + dist_coeffs.end()}; + + camera_model.intrinsic_matrix = Eigen::Matrix(matrix.data()); + camera_model.distortion_coefficients = + Eigen::Matrix(distortion.data()); + camera_model.avg_reprojection_error = repError; + return 0; +} + +int cameracalibration::calibrate(const std::string& input_video, + CameraModel& camera_model, float square_width, + float marker_width, int board_width, + int board_height, double imagerWidthPixels, + double imagerHeightPixels, + bool show_debug_window) { + // ChArUco Board + cv::aruco::Dictionary aruco_dict = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); + cv::Ptr charuco_board = new cv::aruco::CharucoBoard( + cv::Size(board_width, board_height), square_width * 0.0254, + marker_width * 0.0254, aruco_dict); + cv::aruco::CharucoDetector charuco_detector(*charuco_board); + + // Video capture + cv::VideoCapture video_capture(input_video); + cv::Size frame_shape; + + // Detection output + std::vector observation_boards; + std::vector frames_rt_toref; + + cv::Size boardSize(board_width - 1, board_height - 1); + cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels); + + while (video_capture.grab()) { + cv::Mat frame; + video_capture.retrieve(frame); + + cv::Mat debug_image = frame; + + if (frame.empty()) { + break; + } + + // Detect + cv::Mat frame_gray; + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + frame_shape = frame_gray.size(); + + std::vector charuco_corners; + std::vector charuco_ids; + std::vector> marker_corners; + std::vector marker_ids; + + std::vector obj_points; + std::vector img_points; + + std::vector points((board_width - 1) * (board_height - 1)); + + charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, + marker_corners, marker_ids); + + if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, + board_width, board_height)) { + continue; + } + + charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, + img_points); + + for (int i = 0; i < charuco_ids.size(); i++) { + int id = charuco_ids.at(i); + points[id].x = img_points.at(i).x; + points[id].y = img_points.at(i).y; + points[id].z = 1.0f; + } + + for (int i = 0; i < points.size(); i++) { + if (points[i].z != 1.0f) { + points[i].x = -1.0f; + points[i].y = -1.0f; + points[i].z = -1.0f; + } + } + + frames_rt_toref.push_back( + getSeedPose(points.data(), boardSize, imagerSize, square_width, 1000)); + observation_boards.insert(observation_boards.end(), points.begin(), + points.end()); + + if (show_debug_window) { + cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); + cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, + charuco_ids); + cv::imshow("Frame", debug_image); + if (cv::waitKey(1) == 'q') { + break; + } + } + } + + video_capture.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + if (observation_boards.empty()) { + std::cout << "calibration failed" << std::endl; + return 1; + } else { + std::cout << "points detected" << std::endl; + } + + std::unique_ptr cal_result = + mrcal_main(observation_boards, frames_rt_toref, boardSize, + square_width * 0.0254, imagerSize, 1000); + + auto& stats = *cal_result; + + // Camera matrix and distortion coefficients + std::vector camera_matrix = { + // fx 0 cx + stats.intrinsics[0], 0, stats.intrinsics[2], + // 0 fy cy + 0, stats.intrinsics[1], stats.intrinsics[3], + // 0 0 1 + 0, 0, 1}; + + std::vector distortion_coefficients = {stats.intrinsics[4], + stats.intrinsics[5], + stats.intrinsics[6], + stats.intrinsics[7], + stats.intrinsics[8], + stats.intrinsics[9], + stats.intrinsics[10], + stats.intrinsics[11], + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0}; + + camera_model.intrinsic_matrix = + Eigen::Matrix(camera_matrix.data()); + camera_model.distortion_coefficients = + Eigen::Matrix(distortion_coefficients.data()); + camera_model.avg_reprojection_error = stats.rms_error; + + return 0; +} + +int cameracalibration::calibrate(const std::string& input_video, + CameraModel& camera_model, float square_width, + int board_width, int board_height, + double imagerWidthPixels, + double imagerHeightPixels, + bool show_debug_window) { + // Video capture + cv::VideoCapture video_capture(input_video); + + // Detection output + std::vector observation_boards; + std::vector frames_rt_toref; + + cv::Size boardSize(board_width - 1, board_height - 1); + cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels); + + while (video_capture.grab()) { + cv::Mat frame; + video_capture.retrieve(frame); + + if (frame.empty()) { + break; + } + + // Detect + cv::Mat frame_gray; + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + std::vector corners; + + bool found = cv::findChessboardCorners( + frame_gray, boardSize, corners, + cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE); + + if (found) { + std::vector current_points; + for (int i = 0; i < corners.size(); i++) { + current_points.push_back( + mrcal_point3_t{{corners.at(i).x, corners.at(i).y, 1.0f}}); + } + frames_rt_toref.push_back(getSeedPose(current_points.data(), boardSize, + imagerSize, square_width * 0.0254, + 1000)); + observation_boards.insert(observation_boards.end(), + current_points.begin(), current_points.end()); + } + if (show_debug_window) { + cv::drawChessboardCorners(frame, boardSize, corners, found); + cv::imshow("Checkerboard Detection", frame); + if (cv::waitKey(30) == 'q') { + break; + } + } + } + + video_capture.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + if (observation_boards.empty()) { + std::cout << "calibration failed" << std::endl; + return 1; + } else { + std::cout << "points detected" << std::endl; + } + + std::unique_ptr cal_result = + mrcal_main(observation_boards, frames_rt_toref, boardSize, + square_width * 0.0254, imagerSize, 1000); + + auto& stats = *cal_result; + + // Camera matrix and distortion coefficients + std::vector camera_matrix = { + // fx 0 cx + stats.intrinsics[0], 0, stats.intrinsics[2], + // 0 fy cy + 0, stats.intrinsics[1], stats.intrinsics[3], + // 0 0 1 + 0, 0, 1}; + + std::vector distortion_coefficients = {stats.intrinsics[4], + stats.intrinsics[5], + stats.intrinsics[6], + stats.intrinsics[7], + stats.intrinsics[8], + stats.intrinsics[9], + stats.intrinsics[10], + stats.intrinsics[11], + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0}; + + // Save calibration output + camera_model.intrinsic_matrix = + Eigen::Matrix(camera_matrix.data()); + camera_model.distortion_coefficients = + Eigen::Matrix(distortion_coefficients.data()); + camera_model.avg_reprojection_error = stats.rms_error; + + return 0; +} diff --git a/wpical/src/main/native/cpp/fieldcalibration.cpp b/wpical/src/main/native/cpp/fieldcalibration.cpp new file mode 100644 index 0000000000..1f3ff6970a --- /dev/null +++ b/wpical/src/main/native/cpp/fieldcalibration.cpp @@ -0,0 +1,599 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "fieldcalibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "apriltag.h" +#include "tag36h11.h" + +struct Pose { + Eigen::Vector3d p; + Eigen::Quaterniond q; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct Constraint { + int id_begin; + int id_end; + Pose t_begin_end; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class PoseGraphError { + public: + explicit PoseGraphError(Pose t_ab_observed) + : m_t_ab_observed(std::move(t_ab_observed)) {} + + template + bool operator()(const T* const p_a_ptr, const T* const q_a_ptr, + const T* const p_b_ptr, const T* const q_b_ptr, + T* residuals_ptr) const { + // Tag A + Eigen::Map> p_a(p_a_ptr); + Eigen::Map> q_a(q_a_ptr); + + // Tag B + Eigen::Map> p_b(p_b_ptr); + Eigen::Map> q_b(q_b_ptr); + + // Rotation between tag A to tag B + Eigen::Quaternion q_a_inverse = q_a.conjugate(); + Eigen::Quaternion q_ab_estimated = q_a_inverse * q_b; + + // Displacement between tag A and tag B in tag A's frame + Eigen::Matrix p_ab_estimated = q_a_inverse * (p_b - p_a); + + // Error between the orientations + Eigen::Quaternion delta_q = + m_t_ab_observed.q.template cast() * q_ab_estimated.conjugate(); + + // Residuals + Eigen::Map> residuals(residuals_ptr); + residuals.template block<3, 1>(0, 0) = + p_ab_estimated - m_t_ab_observed.p.template cast(); + residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec(); + + return true; + } + + static ceres::CostFunction* Create(const Pose& t_ab_observed) { + return new ceres::AutoDiffCostFunction( + new PoseGraphError(t_ab_observed)); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + const Pose m_t_ab_observed; +}; + +const double tagSizeMeters = 0.1651; + +inline cameracalibration::CameraModel load_camera_model(std::string path) { + Eigen::Matrix camera_matrix; + Eigen::Matrix camera_distortion; + + std::ifstream file(path); + + wpi::json json_data; + + try { + json_data = wpi::json::parse(file); + } catch (const wpi::json::parse_error& e) { + std::cout << e.what() << std::endl; + } + + bool isCalibdb = json_data.contains("camera"); + + if (!isCalibdb) { + for (int i = 0; i < camera_matrix.rows(); i++) { + for (int j = 0; j < camera_matrix.cols(); j++) { + camera_matrix(i, j) = + json_data["camera_matrix"][(i * camera_matrix.cols()) + j]; + } + } + + for (int i = 0; i < camera_distortion.rows(); i++) { + for (int j = 0; j < camera_distortion.cols(); j++) { + camera_distortion(i, j) = json_data["distortion_coefficients"] + [(i * camera_distortion.cols()) + j]; + } + } + } else { + for (int i = 0; i < camera_matrix.rows(); i++) { + for (int j = 0; j < camera_matrix.cols(); j++) { + try { + camera_matrix(i, j) = json_data["camera_matrix"][i][j]; + } catch (...) { + camera_matrix(i, j) = json_data["camera_matrix"]["data"] + [(i * camera_matrix.cols()) + j]; + } + } + } + + for (int i = 0; i < camera_distortion.rows(); i++) { + for (int j = 0; j < camera_distortion.cols(); j++) { + try { + camera_distortion(i, j) = + json_data["distortion_coefficients"] + [(i * camera_distortion.cols()) + j]; + } catch (...) { + camera_distortion(i, j) = 0.0; + } + } + } + } + + cameracalibration::CameraModel camera_model{ + camera_matrix, camera_distortion, json_data["avg_reprojection_error"]}; + return camera_model; +} + +inline cameracalibration::CameraModel load_camera_model(wpi::json json_data) { + // Camera matrix + Eigen::Matrix camera_matrix; + + for (int i = 0; i < camera_matrix.rows(); i++) { + for (int j = 0; j < camera_matrix.cols(); j++) { + camera_matrix(i, j) = + json_data["camera_matrix"][(i * camera_matrix.cols()) + j]; + } + } + + // Distortion coefficients + Eigen::Matrix camera_distortion; + + for (int i = 0; i < camera_distortion.rows(); i++) { + for (int j = 0; j < camera_distortion.cols(); j++) { + camera_distortion(i, j) = json_data["distortion_coefficients"] + [(i * camera_distortion.cols()) + j]; + } + } + + cameracalibration::CameraModel camera_model{ + camera_matrix, camera_distortion, json_data["avg_reprojection_error"]}; + return camera_model; +} + +inline std::map load_ideal_map(std::string path) { + std::ifstream file(path); + wpi::json json_data = wpi::json::parse(file); + std::map ideal_map; + + for (const auto& element : json_data["tags"]) { + ideal_map[element["ID"]] = element; + } + + return ideal_map; +} + +Eigen::Matrix get_tag_transform( + std::map& ideal_map, int tag_id) { + Eigen::Matrix transform = + Eigen::Matrix::Identity(); + + transform.block<3, 3>(0, 0) = + Eigen::Quaternion( + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["W"], + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["X"], + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"], + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"]) + .toRotationMatrix(); + + transform(0, 3) = ideal_map[tag_id]["pose"]["translation"]["x"]; + transform(1, 3) = ideal_map[tag_id]["pose"]["translation"]["y"]; + transform(2, 3) = ideal_map[tag_id]["pose"]["translation"]["z"]; + + return transform; +} + +inline Eigen::Matrix estimate_tag_pose( + apriltag_detection_t* tag_detection, + const Eigen::Matrix& camera_matrix, + const Eigen::Matrix& camera_distortion, double tag_size) { + cv::Mat camera_matrix_cv; + cv::Mat camera_distortion_cv; + + cv::eigen2cv(camera_matrix, camera_matrix_cv); + cv::eigen2cv(camera_distortion, camera_distortion_cv); + + std::vector points_2d = { + cv::Point2f(tag_detection->p[0][0], tag_detection->p[0][1]), + cv::Point2f(tag_detection->p[1][0], tag_detection->p[1][1]), + cv::Point2f(tag_detection->p[2][0], tag_detection->p[2][1]), + cv::Point2f(tag_detection->p[3][0], tag_detection->p[3][1])}; + + std::vector points_3d_box_base = { + cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0), + cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)}; + + std::vector r_vec; + std::vector t_vec; + + cv::solvePnP(points_3d_box_base, points_2d, camera_matrix_cv, + camera_distortion_cv, r_vec, t_vec, false, cv::SOLVEPNP_SQPNP); + + cv::Mat r_mat; + cv::Rodrigues(r_vec, r_mat); + + Eigen::Matrix camera_to_tag{ + {r_mat.at(0, 0), r_mat.at(0, 1), r_mat.at(0, 2), + t_vec.at(0)}, + {r_mat.at(1, 0), r_mat.at(1, 1), r_mat.at(1, 2), + t_vec.at(1)}, + {r_mat.at(2, 0), r_mat.at(2, 1), r_mat.at(2, 2), + t_vec.at(2)}, + {0.0, 0.0, 0.0, 1.0}}; + + return camera_to_tag; +} + +inline void draw_tag_cube(cv::Mat& frame, + Eigen::Matrix camera_to_tag, + const Eigen::Matrix& camera_matrix, + const Eigen::Matrix& camera_distortion, + double tag_size) { + cv::Mat camera_matrix_cv; + cv::Mat camera_distortion_cv; + + cv::eigen2cv(camera_matrix, camera_matrix_cv); + cv::eigen2cv(camera_distortion, camera_distortion_cv); + + std::vector points_3d_box_base = { + cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0), + cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)}; + + std::vector points_3d_box_top = { + cv::Point3f(-tag_size / 2.0, tag_size / 2.0, -tag_size), + cv::Point3f(tag_size / 2.0, tag_size / 2.0, -tag_size), + cv::Point3f(tag_size / 2.0, -tag_size / 2.0, -tag_size), + cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, -tag_size)}; + + std::vector points_2d_box_base = { + cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), + cv::Point2f(0.0, 0.0)}; + + std::vector points_2d_box_top = { + cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), + cv::Point2f(0.0, 0.0)}; + + Eigen::Matrix r_vec = camera_to_tag.block<3, 3>(0, 0); + Eigen::Matrix t_vec = camera_to_tag.block<3, 1>(0, 3); + + cv::Mat r_vec_cv; + cv::Mat t_vec_cv; + + cv::eigen2cv(r_vec, r_vec_cv); + cv::eigen2cv(t_vec, t_vec_cv); + + cv::projectPoints(points_3d_box_base, r_vec_cv, t_vec_cv, camera_matrix_cv, + camera_distortion_cv, points_2d_box_base); + cv::projectPoints(points_3d_box_top, r_vec_cv, t_vec_cv, camera_matrix_cv, + camera_distortion_cv, points_2d_box_top); + + for (int i = 0; i < 4; i++) { + cv::Point2f& point_base = points_2d_box_base.at(i); + cv::Point2f& point_top = points_2d_box_top.at(i); + + cv::line(frame, point_base, point_top, cv::Scalar(0, 255, 255), 5); + + int i_next = (i + 1) % 4; + cv::Point2f& point_base_next = points_2d_box_base.at(i_next); + cv::Point2f& point_top_next = points_2d_box_top.at(i_next); + + cv::line(frame, point_base, point_base_next, cv::Scalar(0, 255, 255), 5); + cv::line(frame, point_top, point_top_next, cv::Scalar(0, 255, 255), 5); + } +} + +inline bool process_video_file( + apriltag_detector_t* tag_detector, + const Eigen::Matrix& camera_matrix, + const Eigen::Matrix& camera_distortion, double tag_size, + const std::string& path, + std::map, + Eigen::aligned_allocator>>& poses, + std::vector>& constraints, + bool show_debug_window) { + if (show_debug_window) { + cv::namedWindow("Processing Frame", cv::WINDOW_NORMAL); + } + cv::VideoCapture video_input(path); + + if (!video_input.isOpened()) { + std::cout << "Unable to open video " << path << std::endl; + return false; + } + + cv::Mat frame; + cv::Mat frame_gray; + cv::Mat frame_debug; + + int frame_num = 0; + + while (video_input.read(frame)) { + std::cout << "Processing " << path << " - Frame " << frame_num++ + << std::endl; + + // Convert color frame to grayscale frame + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + // Clone color frame for debugging + frame_debug = frame.clone(); + + // Detect tags + image_u8_t tag_image = {frame_gray.cols, frame_gray.rows, frame_gray.cols, + frame_gray.data}; + zarray_t* tag_detections = + apriltag_detector_detect(tag_detector, &tag_image); + + // Skip this loop if there are no tags detected + if (zarray_size(tag_detections) == 0) { + std::cout << "No tags detected" << std::endl; + continue; + } + + // Find detection with the smallest tag ID + apriltag_detection_t* tag_detection_min = nullptr; + zarray_get(tag_detections, 0, &tag_detection_min); + + for (int i = 0; i < zarray_size(tag_detections); i++) { + apriltag_detection_t* tag_detection_i; + zarray_get(tag_detections, i, &tag_detection_i); + + if (tag_detection_i->id < tag_detection_min->id) { + tag_detection_min = tag_detection_i; + } + } + + Eigen::Matrix camera_to_tag_min = estimate_tag_pose( + tag_detection_min, camera_matrix, camera_distortion, tag_size); + + // Find transformation from smallest tag ID + for (int i = 0; i < zarray_size(tag_detections); i++) { + apriltag_detection_t* tag_detection_i; + zarray_get(tag_detections, i, &tag_detection_i); + + // Add tag ID to poses + if (poses.find(tag_detection_i->id) == poses.end()) { + poses[tag_detection_i->id] = {Eigen::Vector3d(0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)}; + } + + // Estimate camera to tag pose + Eigen::Matrix caamera_to_tag = estimate_tag_pose( + tag_detection_i, camera_matrix, camera_distortion, tag_size); + + // Draw debug cube + if (show_debug_window) { + draw_tag_cube(frame_debug, caamera_to_tag, camera_matrix, + camera_distortion, tag_size); + } + + // Skip finding transformation from smallest tag ID to itself + if (tag_detection_i->id == tag_detection_min->id) { + continue; + } + + Eigen::Matrix tag_min_to_tag = + camera_to_tag_min.inverse() * caamera_to_tag; + + // Constraint + Constraint constraint; + constraint.id_begin = tag_detection_min->id; + constraint.id_end = tag_detection_i->id; + constraint.t_begin_end.p = tag_min_to_tag.block<3, 1>(0, 3); + constraint.t_begin_end.q = + Eigen::Quaterniond(tag_min_to_tag.block<3, 3>(0, 0)); + + constraints.push_back(constraint); + } + + apriltag_detections_destroy(tag_detections); + + // Show debug + if (show_debug_window) { + cv::imshow("Processing Frame", frame_debug); + cv::waitKey(1); + } + } + + video_input.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + return true; +} + +int fieldcalibration::calibrate(std::string input_dir_path, + std::string output_file_path, + std::string camera_model_path, + std::string ideal_map_path, int pinned_tag_id, + bool show_debug_window) { + // Silence OpenCV logging + cv::utils::logging::setLogLevel( + cv::utils::logging::LogLevel::LOG_LEVEL_SILENT); + + // Load camera model + Eigen::Matrix3d camera_matrix; + Eigen::Matrix camera_distortion; + + try { + auto camera_model = load_camera_model(camera_model_path); + camera_matrix = camera_model.intrinsic_matrix; + camera_distortion = camera_model.distortion_coefficients; + } catch (...) { + return 1; + } + + wpi::json json = wpi::json::parse(std::ifstream(ideal_map_path)); + if (!json.contains("tags")) { + return 1; + } + + // Load ideal field map + std::map ideal_map; + try { + ideal_map = load_ideal_map(ideal_map_path); + } catch (...) { + return 1; + } + + // Apriltag detector + apriltag_detector_t* tag_detector = apriltag_detector_create(); + tag_detector->nthreads = 8; + + apriltag_family_t* tag_family = tag36h11_create(); + apriltag_detector_add_family(tag_detector, tag_family); + + // Find tag poses + std::map, + Eigen::aligned_allocator>> + poses; + std::vector> constraints; + + for (const auto& entry : + std::filesystem::directory_iterator(input_dir_path)) { + if (entry.path().filename().string()[0] == '.') { + continue; + } + + const std::string path = entry.path().string(); + + bool success = process_video_file(tag_detector, camera_matrix, + camera_distortion, tagSizeMeters, path, + poses, constraints, show_debug_window); + + if (!success) { + std::cout << "Unable to process video " << path << std::endl; + return 1; + } + } + + // Build optimization problem + ceres::Problem problem; + ceres::Manifold* quaternion_manifold = new ceres::EigenQuaternionManifold; + + for (const auto& constraint : constraints) { + auto pose_begin_iter = poses.find(constraint.id_begin); + auto pose_end_iter = poses.find(constraint.id_end); + + ceres::CostFunction* cost_function = + PoseGraphError::Create(constraint.t_begin_end); + + problem.AddResidualBlock(cost_function, nullptr, + pose_begin_iter->second.p.data(), + pose_begin_iter->second.q.coeffs().data(), + pose_end_iter->second.p.data(), + pose_end_iter->second.q.coeffs().data()); + + problem.SetManifold(pose_begin_iter->second.q.coeffs().data(), + quaternion_manifold); + problem.SetManifold(pose_end_iter->second.q.coeffs().data(), + quaternion_manifold); + } + + // Pin tag + auto pinned_tag_iter = poses.find(pinned_tag_id); + if (pinned_tag_iter != poses.end()) { + problem.SetParameterBlockConstant(pinned_tag_iter->second.p.data()); + problem.SetParameterBlockConstant( + pinned_tag_iter->second.q.coeffs().data()); + } + + // Solve + ceres::Solver::Options options; + options.max_num_iterations = 200; + options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + options.num_threads = 10; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + std::cout << summary.BriefReport() << std::endl; + + // Output + std::map observed_map = ideal_map; + + Eigen::Matrix correction_a; + correction_a << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; + + Eigen::Matrix correction_b; + correction_b << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1; + + Eigen::Matrix pinned_tag_transform = + get_tag_transform(ideal_map, pinned_tag_id); + + for (const auto& [tag_id, pose] : poses) { + // Transformation from pinned tag + Eigen::Matrix transform = + Eigen::Matrix::Identity(); + + transform.block<3, 3>(0, 0) = pose.q.toRotationMatrix(); + transform.block<3, 1>(0, 3) = pose.p; + + // Transformation from world + Eigen::Matrix corrected_transform = + pinned_tag_transform * correction_a * transform * correction_b; + Eigen::Quaternion corrected_transform_q( + corrected_transform.block<3, 3>(0, 0)); + + observed_map[tag_id]["pose"]["translation"]["x"] = + corrected_transform(0, 3); + observed_map[tag_id]["pose"]["translation"]["y"] = + corrected_transform(1, 3); + observed_map[tag_id]["pose"]["translation"]["z"] = + corrected_transform(2, 3); + + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["X"] = + corrected_transform_q.x(); + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"] = + corrected_transform_q.y(); + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"] = + corrected_transform_q.z(); + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["W"] = + corrected_transform_q.w(); + } + + wpi::json observed_map_json; + + for (const auto& [tag_id, tag_json] : observed_map) { + observed_map_json["tags"].push_back(tag_json); + } + + observed_map_json["field"] = { + {"length", static_cast(json.at("field").at("length"))}, + {"width", static_cast(json.at("field").at("width"))}}; + + std::ofstream output_file(output_file_path); + output_file << observed_map_json.dump(4) << std::endl; + + return 0; +} diff --git a/wpical/src/main/native/cpp/fmap.cpp b/wpical/src/main/native/cpp/fmap.cpp new file mode 100644 index 0000000000..273c9ebe5f --- /dev/null +++ b/wpical/src/main/native/cpp/fmap.cpp @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "fmap.h" + +#include +#include + +wpi::json fmap::singleTag(int tag, const tag::Pose& tagpose) { + std::vector transform = {}; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + transform.push_back(tagpose.transformMatrixFmap(i, j)); + } + } + + return {{"family", "apriltag3_36h11_classic"}, + {"id", tag}, + {"size", 165.1}, + {"transform", transform}, + {"unique", true}}; +} + +wpi::json fmap::convertfmap(const wpi::json& json) { + std::string fmapstart = "{\"fiducials\":["; + + std::string fmapend = "],\"type\":\"frc\"}"; + + Fieldmap fieldmap(json); + + for (int i = 0; i < fieldmap.getNumTags(); i++) { + fmapstart += singleTag(i + 1, fieldmap.getTag(i + 1)).dump(); + if (i != fieldmap.getNumTags() - 1) { + fmapstart += ","; + } + } + + return wpi::json::parse(fmapstart.append(fmapend)); +} diff --git a/wpical/src/main/native/cpp/tagpose.cpp b/wpical/src/main/native/cpp/tagpose.cpp new file mode 100644 index 0000000000..761bf89573 --- /dev/null +++ b/wpical/src/main/native/cpp/tagpose.cpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +namespace tag { +Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y, + double z, double field_length_meters, double field_width_meters) { + xPos = xpos; + yPos = ypos; + zPos = zpos; + quaternion = Eigen::Quaterniond(w, x, y, z); + rotationMatrix = quaternion.toRotationMatrix(); + transformMatrixFmap.setZero(); + transformMatrixFmap.block<3, 3>(0, 0) = rotationMatrix; + transformMatrixFmap(0, 3) = xpos - (field_length_meters / 2.0); + transformMatrixFmap(1, 3) = ypos - (field_width_meters / 2.0); + transformMatrixFmap(2, 3) = zpos; + transformMatrixFmap(3, 3) = 1; + transformMatrixFmap(3, 0) = 0; + transformMatrixFmap(3, 1) = 0; + transformMatrixFmap(3, 2) = 0; + Eigen::Vector3d eulerAngles = rotationMatrix.eulerAngles(0, 1, 2); + rollRot = eulerAngles[0]; + pitchRot = eulerAngles[1]; + yawRot = eulerAngles[2]; +} +} // namespace tag diff --git a/wpical/src/main/native/include/cameracalibration.h b/wpical/src/main/native/include/cameracalibration.h new file mode 100644 index 0000000000..bd2744d565 --- /dev/null +++ b/wpical/src/main/native/include/cameracalibration.h @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include +#include + +namespace cameracalibration { +struct CameraModel { + Eigen::Matrix intrinsic_matrix; + Eigen::Matrix distortion_coefficients; + double avg_reprojection_error; +}; + +int calibrate(const std::string& input_video, CameraModel& camera_model, + float square_width, float marker_width, int board_width, + int board_height, bool show_debug_window); +int calibrate(const std::string& input_video, CameraModel& camera_model, + float square_width, float marker_width, int board_width, + int board_height, double imagerWidthPixels, + double imagerHeightPixels, bool show_debug_window); +int calibrate(const std::string& input_video, CameraModel& camera_model, + float square_width, int board_width, int board_height, + double imagerWidthPixels, double imagerHeightPixels, + bool show_debug_window); +static void dumpJson(CameraModel& camera_model, + const std::string& output_file_path) { + std::vector camera_matrix(camera_model.intrinsic_matrix.data(), + camera_model.intrinsic_matrix.data() + + camera_model.intrinsic_matrix.size()); + std::vector distortion_coefficients( + camera_model.distortion_coefficients.data(), + camera_model.distortion_coefficients.data() + + camera_model.distortion_coefficients.size()); + + wpi::json result = { + {"camera_matrix", camera_matrix}, + {"distortion_coefficients", distortion_coefficients}, + {"avg_reprojection_error", camera_model.avg_reprojection_error}}; + + std::ofstream output_file(output_file_path); + output_file << result.dump(4) << std::endl; + output_file.close(); +} +} // namespace cameracalibration diff --git a/wpical/src/main/native/include/fieldcalibration.h b/wpical/src/main/native/include/fieldcalibration.h new file mode 100644 index 0000000000..d9de45990b --- /dev/null +++ b/wpical/src/main/native/include/fieldcalibration.h @@ -0,0 +1,15 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "cameracalibration.h" + +namespace fieldcalibration { +int calibrate(std::string input_dir_path, std::string output_file_path, + std::string camera_model_path, std::string ideal_map_path, + int pinned_tag_id, bool show_debug_window); +} // namespace fieldcalibration diff --git a/wpical/src/main/native/include/fieldmap.h b/wpical/src/main/native/include/fieldmap.h new file mode 100644 index 0000000000..22fefc28a4 --- /dev/null +++ b/wpical/src/main/native/include/fieldmap.h @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include +#include + +class Fieldmap { + public: + Fieldmap() = default; + explicit Fieldmap(const wpi::json& json) { + double field_length_meters = + static_cast(json.at("field").at("length")); + double field_width_meters = + static_cast(json.at("field").at("width")); + for (const auto& tag : json.at("tags").items()) { + double tagXPos = + static_cast(tag.value().at("pose").at("translation").at("x")); + double tagYPos = + static_cast(tag.value().at("pose").at("translation").at("y")); + double tagZPos = + static_cast(tag.value().at("pose").at("translation").at("z")); + double tagWQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("W")); + double tagXQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("X")); + double tagYQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("Y")); + double tagZQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("Z")); + + tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat, + tagYQuat, tagZQuat, field_length_meters, + field_width_meters); + } + } + + const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; } + + int getNumTags() const { return tagVec.size(); } + + static double minimizeAngle(double angle) { + angle = std::fmod(angle, 360); + if (angle > 180) { + return angle - 360; + } else if (angle < -180) { + return angle + 360; + } + return angle; + } + + private: + std::vector tagVec; +}; diff --git a/wpical/src/main/native/include/fmap.h b/wpical/src/main/native/include/fmap.h new file mode 100644 index 0000000000..6757b7cada --- /dev/null +++ b/wpical/src/main/native/include/fmap.h @@ -0,0 +1,15 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +namespace fmap { +wpi::json singleTag(int tag, const tag::Pose& tagpose); +wpi::json convertfmap(const wpi::json& json); +} // namespace fmap diff --git a/wpical/src/main/native/include/tagpose.h b/wpical/src/main/native/include/tagpose.h new file mode 100644 index 0000000000..422fc76a93 --- /dev/null +++ b/wpical/src/main/native/include/tagpose.h @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +namespace tag { +class Pose { + public: + Pose(double xpos, double ypos, double zpos, double w, double x, double y, + double z, double field_length_meters, double field_width_meters); + double xPos, yPos, zPos, yawRot, rollRot, pitchRot; + Eigen::Quaterniond quaternion; + Eigen::Matrix3d rotationMatrix; + Eigen::Matrix4d transformMatrixFmap; +}; +} // namespace tag diff --git a/wpical/src/main/native/mac/wpical.icns b/wpical/src/main/native/mac/wpical.icns new file mode 100644 index 0000000000..06015595be Binary files /dev/null and b/wpical/src/main/native/mac/wpical.icns differ diff --git a/wpical/src/main/native/resources/2024-crescendo.json b/wpical/src/main/native/resources/2024-crescendo.json new file mode 100644 index 0000000000..8cb837a502 --- /dev/null +++ b/wpical/src/main/native/resources/2024-crescendo.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/wpical/src/main/native/resources/altfieldvideo/long.avi b/wpical/src/main/native/resources/altfieldvideo/long.avi new file mode 100644 index 0000000000..5df4dd35ce Binary files /dev/null and b/wpical/src/main/native/resources/altfieldvideo/long.avi differ diff --git a/wpical/src/main/native/resources/altfieldvideo/short.avi b/wpical/src/main/native/resources/altfieldvideo/short.avi new file mode 100644 index 0000000000..8a5d28e2f3 Binary files /dev/null and b/wpical/src/main/native/resources/altfieldvideo/short.avi differ diff --git a/wpical/src/main/native/resources/fieldvideo/long.mp4 b/wpical/src/main/native/resources/fieldvideo/long.mp4 new file mode 100644 index 0000000000..523ab0c869 Binary files /dev/null and b/wpical/src/main/native/resources/fieldvideo/long.mp4 differ diff --git a/wpical/src/main/native/resources/fieldvideo/short.mp4 b/wpical/src/main/native/resources/fieldvideo/short.mp4 new file mode 100644 index 0000000000..0d1643a5c5 Binary files /dev/null and b/wpical/src/main/native/resources/fieldvideo/short.mp4 differ diff --git a/wpical/src/main/native/resources/lifecam_1280p_10x10.vnl b/wpical/src/main/native/resources/lifecam_1280p_10x10.vnl new file mode 100644 index 0000000000..4db21a5862 --- /dev/null +++ b/wpical/src/main/native/resources/lifecam_1280p_10x10.vnl @@ -0,0 +1,1204 @@ +## generated with mrgingham --jobs 4 --gridn 10 /home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/*.png +# filename x y level +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.015437 296.974271 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.509411 297.531269 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.520930 297.109302 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.075949 297.800105 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 425.940824 297.621011 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 467.313812 298.319337 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 509.130579 298.103306 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 548.803849 298.629423 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 589.443478 298.758103 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 628.032861 299.023840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.154750 340.004086 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.798148 339.659259 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.652445 339.811122 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.427900 339.400470 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 425.527248 339.720027 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 467.897135 338.868490 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 508.870179 339.166510 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 549.899648 338.867958 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 589.019359 339.137326 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 628.936450 338.489408 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.865169 383.082397 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.690674 382.433679 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.837000 382.513000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.269743 381.424135 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.337838 380.901138 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 467.986314 380.531478 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 509.646416 379.907655 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 549.761326 379.574498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 590.184535 378.994802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 628.631792 378.915653 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.854308 426.266699 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.331000 425.828000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.680823 424.368350 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.214677 423.507119 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.182903 422.578032 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.320212 421.791824 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 509.632653 420.944341 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 550.398293 420.147937 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 589.803175 419.660317 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 629.710870 418.489855 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.721127 469.678873 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.393652 468.568925 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.620690 467.210345 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.173438 465.995755 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.304864 464.772242 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.503483 463.320398 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.123644 462.287419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 550.344845 461.222990 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 590.841542 459.922912 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 629.821138 459.304201 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.821820 512.940448 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.325482 511.647752 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.535069 510.021785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.162100 508.515982 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.202491 506.836492 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.304000 505.577000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.045658 503.621307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.054721 502.112661 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 590.939336 500.918957 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 630.756393 499.445491 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.044590 556.417705 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.448980 554.871234 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.700613 553.239264 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.161168 551.239589 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.261317 549.263374 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.541840 547.375668 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.198598 545.350467 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.186118 543.658267 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 591.562349 541.892196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 630.761721 540.378292 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.693449 599.738997 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.819931 598.087973 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.866451 596.038817 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.134593 593.874852 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.277143 591.888571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.548673 589.714602 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.568214 587.343647 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.565421 585.352336 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 591.894858 583.191001 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 631.611511 581.210432 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 252.027027 642.986486 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 296.374491 641.043419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 340.162115 638.979663 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.585432 636.766187 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.407666 634.381533 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 469.002996 631.869196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.772475 629.584906 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.706190 627.051874 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 592.329350 624.628827 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 632.000646 622.551969 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 252.861689 686.089963 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 296.932812 683.951220 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 340.908951 681.604938 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.883708 679.330773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.932635 676.690487 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 469.262226 674.123946 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 511.052474 671.581209 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 552.267148 668.786101 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 592.656385 666.170265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 632.669465 663.506821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img9.png - - - +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 33.725373 205.770576 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 66.154079 184.195368 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 97.410349 163.550485 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 128.525066 142.348945 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 158.456728 122.444725 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 188.364369 102.185318 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 217.360747 82.825605 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 246.251193 63.495011 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 274.418169 44.810333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 302.566722 26.268140 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 58.761305 235.412587 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 90.082251 213.942058 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 120.679173 192.735931 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 150.441873 172.412672 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 179.994505 151.795971 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 208.859013 132.026452 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 237.811258 112.350993 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 265.906844 93.376847 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 294.007736 74.433225 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 321.426879 55.879674 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 82.707277 264.083635 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 113.103559 242.659736 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 142.748059 221.957175 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 172.115163 201.362124 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 201.110328 180.915188 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 229.719728 160.984911 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 257.570457 141.494015 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 285.790958 122.426971 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 313.028417 103.270863 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 340.123120 84.679511 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 106.046612 292.229268 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 135.189369 270.904485 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 164.295097 249.963563 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 193.135522 229.392684 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 221.777822 209.050079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 249.566908 189.174155 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 277.511446 169.474595 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 304.610937 150.516028 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 331.936703 131.619763 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 358.203623 113.219565 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 127.785714 319.432773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 156.894940 298.478566 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 185.743784 277.343627 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 213.865377 256.787653 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 241.777546 236.643136 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 269.746660 216.476876 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 296.589336 197.439040 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 323.627969 178.178161 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 350.044477 159.458987 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 376.129792 140.651757 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 149.621249 345.784063 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 178.128072 324.861810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 206.406940 303.860673 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 234.122778 283.522286 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 261.916356 263.113479 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 288.846795 243.799359 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 315.758399 224.280861 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 341.913787 205.187239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 367.880603 186.671559 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 393.244861 168.002418 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 170.640172 371.641894 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 198.878871 350.450412 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 226.583950 329.922880 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 254.119098 309.396406 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 281.142932 289.523822 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 307.732691 269.647239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 334.136618 250.477044 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 360.002450 231.540427 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 385.484971 212.754881 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 410.349912 194.686165 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 191.637540 396.382802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 219.106787 375.773039 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 246.427054 355.399815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 273.498347 334.846428 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 300.042506 314.870694 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 326.327887 295.231481 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 352.108108 275.892764 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 377.401973 257.078601 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 402.526776 238.664683 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 427.084655 220.048131 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 211.902338 421.048831 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 238.945342 400.331677 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 265.966763 379.784915 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 292.438931 359.509815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 318.435282 339.733920 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 344.310945 319.956882 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 369.622007 300.966123 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 394.517337 281.905340 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 418.951189 263.451564 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 443.188438 245.154583 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 231.733915 444.557797 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 258.128422 424.035147 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 284.693299 403.438660 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 310.522819 383.378188 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 336.524351 363.578516 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 361.747836 344.118869 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 386.717939 325.015043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 411.235345 306.066379 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 435.076995 287.761565 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 458.851006 269.592985 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img10.png - - - +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 606.166809 46.390932 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 656.527620 46.850567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 706.010458 47.189542 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 757.207534 47.724947 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 807.846749 48.552632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 859.281225 49.107977 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 911.102866 49.145400 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 963.236636 50.190456 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1014.831401 51.401449 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1067.427916 52.475098 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 605.264291 96.454343 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 654.468647 96.831683 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 705.278589 97.519465 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 755.314832 98.377616 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 806.522032 98.916157 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 857.656762 99.932377 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 909.635977 100.607649 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 961.979004 101.385916 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1013.547583 102.751511 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1065.998519 103.302222 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 603.733168 146.240099 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 653.628066 147.156874 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 703.453562 147.981552 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 754.398295 148.514773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 804.948846 149.804741 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 856.542794 150.568747 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 908.201954 151.724756 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 960.133838 152.617845 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1012.452335 153.964008 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1064.578110 155.364827 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 602.391832 196.346522 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 652.234165 197.322457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 702.178635 198.203561 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 752.800348 199.476494 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 803.742871 200.011116 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 855.181818 201.379203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 906.929845 202.237812 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 958.951652 203.771152 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1011.432907 204.949681 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1063.287671 206.328767 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 601.807096 245.722639 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 650.847842 247.186540 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 700.983157 248.198749 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 751.222801 249.400651 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 802.368288 250.838399 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 853.912192 251.829978 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 905.725225 253.335586 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 957.935957 254.438272 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1010.135156 256.060156 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1062.360485 257.584922 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 600.282069 295.565517 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 650.052062 296.625773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 699.639273 298.371636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 750.259868 299.569079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 801.055085 301.189831 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 852.506148 302.597848 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 904.786545 303.769103 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 956.673050 305.581560 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1009.289588 307.042299 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1061.354478 308.680970 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 599.869613 344.411893 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 648.723684 346.445395 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 698.977373 347.929169 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 748.888889 349.864734 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 799.983445 351.273668 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 851.732323 352.974747 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 903.575980 354.798203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 956.156646 356.839732 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1007.875415 358.427741 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1060.383920 360.523618 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 598.545455 393.875077 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 648.323441 395.658451 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 697.673737 398.089899 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 748.243056 399.447650 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 799.241703 401.868687 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 850.523416 403.607897 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 902.858505 405.951564 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 954.901249 407.348468 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1007.203402 410.039941 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1059.151685 411.540262 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 598.282490 443.125171 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 647.082897 445.513089 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 697.187537 447.427984 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 747.639900 449.829549 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 798.230889 451.925117 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 850.044925 454.290349 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 902.246975 456.569307 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 954.056077 458.850918 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1005.688801 460.529237 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1057.716944 463.236043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 597.068257 492.785951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 646.740648 494.796758 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 696.014354 497.460925 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 746.782009 499.701904 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 797.676649 502.355193 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 848.888179 505.039936 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 900.846221 507.467487 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 952.767794 509.476868 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1004.449529 511.932768 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1056.767989 513.663080 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 782.867536 169.829905 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 818.491897 169.535113 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 854.664952 169.623806 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 892.484076 169.244161 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 931.294433 169.122056 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 971.671227 168.894969 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1013.042029 169.194203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1055.847869 168.886898 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1099.728412 169.026462 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1145.835689 168.867491 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 775.105051 202.570909 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 810.597612 202.549325 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 847.598015 202.928394 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 885.750000 203.705214 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 924.999516 203.571221 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 965.645998 204.631342 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1007.401820 204.476054 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1050.470414 205.717949 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1094.879512 206.100976 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1140.859076 206.514092 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 767.005135 235.436457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 802.731218 236.495259 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 840.180576 237.517986 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 878.488688 238.110407 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 918.083107 239.439024 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 959.328034 239.794913 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1001.322940 241.652561 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1044.964749 242.668740 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1089.825267 243.881835 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1136.322478 245.256553 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 758.636455 269.463914 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 794.995806 270.916589 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 832.236567 272.221912 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 871.048208 273.807166 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 911.203020 275.191275 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 952.528331 277.631700 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 995.293895 278.685575 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1039.214830 281.101178 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1084.627053 282.897101 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1131.438961 284.918831 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 750.547022 303.710554 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 786.582459 305.905922 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 824.476544 308.036357 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 863.617103 310.011941 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 903.965461 312.654605 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 945.990734 314.353528 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 989.002367 317.686982 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1033.621053 319.661842 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1079.253313 322.643731 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1126.687663 325.572098 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 741.817769 338.821901 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 778.679182 341.639033 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 816.429469 344.210628 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 855.936416 347.442004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 896.611305 349.902098 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 938.767515 353.621135 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 982.712382 356.445925 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1027.277246 360.217054 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1073.893966 363.478617 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1121.521008 366.973307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 733.442032 374.528156 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 769.976058 378.230868 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 808.483760 381.615066 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 847.925278 385.229465 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 889.156909 389.000000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 931.895811 392.650913 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 975.664336 396.996066 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1021.358200 401.270037 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1068.103803 405.463087 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1116.666849 410.081510 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 724.307132 411.267569 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 761.740126 415.194159 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 800.158153 419.517145 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 840.108795 424.116567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 882.041629 428.909579 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 924.442083 433.498333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 969.306030 438.078498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1014.853322 443.477998 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1062.441116 448.650783 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1111.199012 453.765926 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 715.837162 448.473196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 752.785714 453.388095 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 791.984791 458.620118 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 831.847102 463.937115 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 873.692565 469.241855 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 917.302454 475.053129 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 962.032161 480.855779 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1008.687332 486.738544 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1056.092561 492.676038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1105.695515 499.014949 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 706.450704 486.927920 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 744.318410 492.652720 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 782.853237 498.489209 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 823.996278 504.791961 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 865.568293 511.136585 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 909.784570 517.771897 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 954.849589 524.441246 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1001.489270 531.162661 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1049.652520 537.889920 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1099.589183 544.955696 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 121.979212 397.623332 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 154.234215 384.833904 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 186.240070 372.383972 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 218.342648 359.526655 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 250.123647 346.934638 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 282.377762 334.413329 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 313.852881 321.967307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 345.625841 310.080305 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 377.059165 297.598548 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 408.266254 285.674149 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 129.609380 426.542594 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 162.176794 413.403038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 194.624288 400.428843 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 226.969150 387.659518 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 259.479292 375.035015 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 291.783984 362.117864 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 324.111341 349.403309 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 355.974290 336.896482 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 387.597172 324.660986 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 418.968231 312.446931 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 137.515957 455.789229 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 170.400061 442.923053 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 203.207675 429.602333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 236.037206 416.699413 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 268.759211 403.530702 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 301.486884 390.576775 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 334.101487 377.486002 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 366.474394 364.816712 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 398.492320 351.886675 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 430.256529 339.698821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 145.686379 486.115191 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 178.827852 472.785816 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 212.116682 459.195989 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 245.317316 446.011599 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 278.341826 432.707006 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 311.598278 419.381636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 344.366949 406.292453 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 377.374558 392.953180 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 409.553118 380.284450 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 441.792655 367.208875 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 153.727603 517.052562 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 187.542516 503.542516 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 221.365761 489.920362 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 254.803787 476.139759 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 288.256820 462.507526 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 321.718111 449.051692 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 355.029475 435.446409 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 388.082550 422.281087 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 420.890080 408.740483 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 453.220347 395.951642 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 162.616755 548.635198 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 196.489263 534.861895 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 230.412844 521.149157 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 264.452710 507.247078 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 298.434146 493.199187 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 332.034197 479.466078 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 365.867841 465.639567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 399.434258 451.711701 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 432.362283 438.394541 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 465.085020 424.657220 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 171.439695 581.272366 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 206.008645 567.090769 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 240.208673 553.169385 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 274.322535 539.028169 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 308.808324 524.817087 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 342.948650 510.595396 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 376.891269 496.498353 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 410.751190 482.531372 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 444.327897 468.267811 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 477.136010 455.092832 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 180.761144 614.267872 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 215.562672 600.041629 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 250.162143 585.801322 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 285.098901 571.389242 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 319.509244 556.979412 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 354.059568 542.571109 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 388.472829 527.968004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 422.576670 513.615240 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 456.315436 499.452796 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 489.626775 485.048682 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 190.351247 648.604041 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 225.453077 633.958061 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 260.679134 619.259843 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 295.534073 604.746038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 330.610646 590.070722 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 365.415020 575.302654 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 400.002642 560.541612 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 434.606607 545.825325 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 468.603000 531.218000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 502.463039 516.826304 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 200.044714 683.410364 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 235.889354 668.278940 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 271.430902 653.536468 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 306.230831 638.618211 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 341.820780 623.786298 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 377.134652 608.760829 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 412.084400 593.885728 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 446.731092 578.758403 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 481.424685 564.025579 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 515.855000 549.312000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 57.017923 108.276267 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 109.170330 108.091575 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 158.693587 107.598575 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 207.898518 107.404789 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 255.446753 106.521429 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 303.272841 106.813517 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 349.777778 106.088670 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 396.056911 106.513550 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 440.713823 106.093389 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 485.233611 107.030989 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 59.065217 154.793951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 110.295855 154.298446 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 159.931925 153.791080 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 209.167264 152.202509 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 257.870777 151.672297 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 305.582759 150.854023 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 353.076210 150.566426 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 398.814795 150.134795 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 444.477273 149.827441 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 488.110587 149.342767 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 60.248267 202.230624 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 112.002841 200.930114 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 161.733968 199.643788 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 211.107090 198.233515 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 259.788820 196.843027 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 308.157859 196.322581 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 355.230907 194.634845 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 401.708108 194.693694 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 447.206934 193.224810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 491.638932 193.073783 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 62.059126 249.616967 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 113.425316 248.122785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 163.688694 246.255034 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 213.052288 244.308972 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 262.218710 243.176991 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 310.566053 240.878303 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 358.299417 240.361633 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 404.820152 238.552004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 450.079123 238.049571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 495.142259 236.086671 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 63.555556 297.713725 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 115.202322 295.542841 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 165.280124 293.231677 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 215.472879 291.152527 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 264.629827 288.807590 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 313.319036 287.476498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 361.149111 285.084815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 407.807479 283.831717 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 453.938076 281.561265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 498.342466 280.391096 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 65.271039 345.940221 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 117.043125 343.607500 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 167.611352 340.939394 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 217.630025 338.346106 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 267.267672 335.870877 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 315.837635 333.105745 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 364.239746 331.052734 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 411.415154 328.447514 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 456.839975 326.671727 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 502.098710 323.989677 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 66.963528 395.124005 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 119.066173 391.892840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 169.750885 388.957497 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 220.089882 385.951529 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 269.708812 382.664112 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 319.022199 380.011358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 367.029445 376.698754 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 414.187100 374.166420 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 460.570122 371.215447 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 505.192217 368.627948 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 68.725576 444.346609 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 121.077020 440.823864 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 172.000983 437.332187 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 222.446483 434.308869 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 272.297284 430.370221 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 321.423131 426.894217 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 369.227000 423.490000 2 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 417.317771 419.884036 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 463.553655 416.605495 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 508.995192 413.396635 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 70.750996 494.146414 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 123.113329 489.906632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 174.225619 486.198208 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 224.839738 482.322399 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 274.711443 478.573134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 324.216676 474.200212 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 372.626673 470.504758 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 420.208080 466.314887 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 466.866412 462.185115 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 511.891787 458.588790 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 72.445783 543.855422 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 125.508403 539.436134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 176.948133 535.271093 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 227.344928 530.886957 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 277.347804 526.661282 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 326.943015 522.340074 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 375.118943 517.720509 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 423.360795 512.988636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 469.516452 508.773486 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 515.495256 504.111954 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 705.620043 247.722937 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 739.210600 248.322176 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 774.388115 249.168059 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 810.185681 249.663476 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 847.396817 250.811745 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 885.823087 251.136763 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 925.543974 252.803474 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 965.825133 253.035239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1007.568932 254.553398 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1050.322643 255.396408 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 698.230305 280.517960 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 732.558824 281.705510 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 767.551150 282.966011 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 804.002510 284.141423 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 841.245686 285.352095 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 879.856936 287.159441 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 919.761838 288.189873 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 960.775623 290.208449 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1003.223062 291.425961 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1046.146510 293.319281 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 691.441190 313.980009 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 725.119297 315.791484 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 761.030758 317.681802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 797.208098 319.303439 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 835.116383 321.349148 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 873.937984 323.039216 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 914.305729 325.448958 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 955.741473 327.302865 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 998.538622 329.832289 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1042.434821 331.861607 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 683.579122 347.654515 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 718.528543 350.231299 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 753.726843 352.489160 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 790.885062 354.925308 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 828.660313 357.383675 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 868.148275 360.269245 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 908.921569 362.551821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 950.583645 365.784112 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 994.051478 368.587226 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1037.787273 371.737576 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 676.642897 382.325594 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 711.144028 385.053084 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 747.205342 388.143600 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 783.905914 391.374104 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 822.399788 394.544008 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 862.101170 397.772195 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 902.899960 401.332543 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 945.513228 404.706349 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 988.755022 408.494978 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1033.716609 412.099884 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 668.686802 417.437056 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 703.870032 420.953455 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 739.725054 424.657809 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 777.338250 428.522779 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 815.754123 432.485007 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 855.784946 436.457587 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 897.050469 440.606808 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 939.821023 445.111742 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 984.008608 449.285509 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1028.850824 453.912471 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 661.042688 453.439983 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 696.107467 457.721563 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 732.756642 461.987537 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 770.118927 466.691806 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 809.114929 471.183658 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 849.560241 475.971386 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 891.055405 481.015752 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 934.347261 486.547039 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 978.430260 491.383452 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1024.112420 496.517131 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 652.845625 490.514375 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 688.509246 495.278450 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 724.713268 500.579697 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 763.133795 505.681371 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 801.951220 511.195122 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 842.912652 516.618317 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 885.031524 522.183888 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 928.234927 528.189882 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 972.971893 533.991124 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1018.765625 540.061655 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 644.942411 528.385268 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 680.302817 533.845070 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 717.444777 539.659675 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 755.544928 545.440580 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 795.084523 551.706915 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 836.451705 558.369886 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 878.449286 564.614576 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 922.164464 571.287812 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 967.234547 577.945385 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1013.380172 584.604310 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 636.103741 567.278061 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 672.448970 573.380931 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 709.133728 580.049704 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 748.202561 586.549611 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 788.161084 593.755748 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 829.400773 600.654273 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 872.000000 607.720524 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 915.974121 615.131015 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 961.087224 622.386978 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1007.670813 629.776100 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 314.486192 98.216570 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 366.072091 98.666323 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 418.123077 98.938462 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 470.366292 99.733708 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 522.096725 99.722772 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 575.288970 100.488605 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 627.017781 101.006401 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 679.894706 101.466984 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 732.232939 102.266606 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 785.083035 102.624195 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 312.686131 150.357664 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 364.409353 150.865473 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 416.518467 151.681533 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 468.606157 151.957537 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 520.889734 152.984791 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 573.074975 153.227905 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 625.393675 153.953108 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 677.724623 154.572618 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 730.284759 155.002389 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 782.881356 155.760401 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 311.185532 202.344943 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 363.233492 203.233492 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 415.315104 203.781250 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 467.371859 204.877243 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 519.609663 205.174810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 571.625229 206.160878 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 623.822760 206.754964 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 676.004479 207.250280 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 728.500000 207.959393 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 781.064906 208.473047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 310.329630 254.632099 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 362.106620 255.252265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 414.134405 256.337621 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 466.458722 256.629161 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 518.277666 257.888665 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 570.656889 258.323329 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 622.190269 259.397967 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 674.834696 259.798481 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 726.742616 260.649789 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 779.682238 261.068051 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 309.083612 306.546265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 361.344330 307.208247 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 413.495829 307.800741 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 465.328814 309.204068 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 517.577677 309.467572 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 569.209145 310.491754 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 621.488746 311.122186 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 673.085630 312.244094 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 725.712546 312.754568 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 778.035461 313.805471 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 308.200650 358.437043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 359.915550 359.692646 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 412.544218 359.815571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 464.650118 360.652482 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 516.315484 361.452258 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 568.500752 362.141353 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 619.775742 363.335079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 672.469197 363.860610 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 724.036364 365.140260 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 776.848564 365.482376 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 306.909361 410.476226 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 358.772139 410.849751 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 411.112385 411.756881 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 463.381761 412.305660 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 515.414903 413.217111 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 566.971347 413.848711 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 619.106396 414.699877 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 670.752232 415.971726 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 722.979565 416.679631 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 775.454194 417.793548 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 305.886707 462.168429 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 357.868908 463.016807 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 409.938882 463.542263 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 461.927856 464.256513 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 514.043390 464.781017 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 565.887518 465.785196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 617.705446 466.717203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 669.721866 467.628571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 721.520219 468.824044 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 774.037820 469.844271 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 304.919127 514.137356 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 356.644820 514.692389 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 408.480306 515.517505 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 460.841808 516.204802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 512.850208 516.919556 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 564.602596 517.673053 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 616.632555 518.684066 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 668.414955 519.732950 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 720.531500 521.000887 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 773.077904 522.406920 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 304.297245 565.508648 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 355.640523 566.789542 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 407.573308 567.480263 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 459.554988 568.419874 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 511.643501 569.311454 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 563.765672 569.874627 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 615.506135 570.830521 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 667.554566 571.897550 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 719.653169 573.140845 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 772.378540 574.432662 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 41.054671 241.910035 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 89.836860 240.488737 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 137.196226 238.977358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 183.766830 237.588739 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 230.269520 236.111111 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 275.676385 235.248785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 320.955771 233.158950 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 365.418006 232.922026 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 409.281481 231.123810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 452.127018 230.574035 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 43.412748 287.188088 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 91.762653 285.762653 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 139.129336 283.641724 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 185.577143 281.928254 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 231.916667 280.222222 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 278.008362 278.287108 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 323.107299 277.202745 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 368.039133 275.124624 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 411.291667 274.256366 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 454.731258 272.249047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 45.198657 333.035814 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 93.640900 330.831703 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 140.931275 328.664531 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 187.706834 326.528951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 234.098039 324.423203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 279.958763 322.360825 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 325.684335 320.034019 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 369.873442 320.038351 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 414.104704 316.112291 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 456.583041 314.822222 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 47.291925 378.508075 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 95.893747 376.141840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 143.179560 373.594870 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 189.764977 371.048716 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 236.096535 368.691584 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 282.146552 365.962644 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 327.766405 363.702742 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 372.440845 361.114085 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 416.354854 358.932139 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 459.289005 356.515183 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 49.538999 424.490105 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 97.927840 421.659218 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 145.238181 418.715141 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 191.971728 415.956680 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 238.368760 413.034622 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 284.381164 410.076430 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 329.893730 407.343252 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 374.481599 404.390731 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 418.370262 401.183673 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 461.153929 399.053821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 51.633628 470.195280 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 100.121766 466.979037 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 147.612570 463.870544 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 194.315882 460.658235 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 240.573017 457.575919 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 286.424479 454.302951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 332.015748 450.878861 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 376.410924 447.801401 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 420.483855 444.368819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 463.542705 440.912811 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 53.784326 516.112226 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 102.507737 512.342535 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 149.931752 508.741045 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 196.735753 505.411397 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 242.827117 502.037300 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 288.752887 498.400693 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 333.986811 495.005995 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 378.656287 490.884841 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 422.846847 487.219219 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 465.697574 483.696496 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 55.962675 562.056506 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 104.969444 557.753241 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 152.511638 553.954342 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 199.053309 550.172181 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 245.209046 546.462918 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 290.900156 542.609204 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 336.176649 538.532970 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 380.961017 534.571186 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 424.822446 530.649079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 467.260500 526.346746 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 58.018914 608.175107 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 107.596023 603.121591 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 154.991018 598.780240 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 201.816667 594.844608 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 247.899312 590.853143 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 293.332249 586.720260 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 338.493681 582.515798 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 382.775449 578.061078 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 426.722704 573.525130 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 469.862947 569.269772 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 60.122248 654.243736 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 109.562130 648.899408 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 157.980249 643.847110 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 204.290011 639.335982 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 250.668072 635.092169 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 295.955821 630.871622 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 340.777215 626.288608 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 385.036058 621.670673 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 428.886332 616.951620 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 472.062500 612.084375 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 133.706282 168.856819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 170.184165 167.489960 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 207.017581 165.463979 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 244.167728 164.148620 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 281.010242 162.474860 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 318.416040 161.093358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 355.165275 159.545353 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 392.870189 158.343118 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 429.452917 156.936029 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 466.855398 156.219152 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 135.152946 205.877193 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 171.510140 203.831513 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 208.890390 202.351688 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 245.454869 201.039984 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 282.694342 199.663141 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 319.885163 197.883785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 357.509848 196.759707 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 394.495886 195.308792 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 431.666831 194.420662 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 468.501857 192.788279 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 136.439798 242.648363 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 173.141715 241.088405 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 210.192908 239.517730 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 247.137202 237.903284 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 284.334732 236.324009 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 321.564878 235.047805 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 358.842318 233.509434 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 396.254801 232.444965 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 433.548990 231.120419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 470.152187 230.112043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 137.913535 279.605587 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 174.488759 277.961331 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 211.520925 276.358709 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 248.623771 274.946131 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 286.012195 273.244841 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 323.296400 272.029200 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 360.710537 270.748667 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 398.044541 269.057154 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 434.881771 268.331470 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 472.176000 266.296000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 139.412893 316.674827 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 176.048926 314.994016 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 213.095325 313.622365 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 250.109457 311.911871 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 287.603843 310.460659 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 324.838323 309.047049 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 362.490097 307.453247 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 399.398363 306.304910 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 436.948902 304.403056 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 473.421693 303.558730 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 140.750204 353.596893 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 177.728705 352.130970 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 214.631292 350.292785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 251.795091 349.025114 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 289.107257 347.350478 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 326.430244 345.752195 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 363.891667 344.216270 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 401.275737 342.562358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 438.111686 341.542906 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 475.841975 339.679835 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 142.512938 390.539390 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 179.259850 388.891182 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 216.246231 387.563819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 253.131245 385.979778 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 290.861300 384.198142 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 327.991193 382.695288 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 365.620509 380.996734 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 402.868987 379.552210 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 440.276580 377.770115 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 476.693449 376.583419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 144.087531 427.275306 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 180.663934 426.045667 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 217.729412 424.384615 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 254.836867 423.037102 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 292.106810 421.210821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 329.793362 419.717345 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 367.082141 417.948508 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 404.424163 416.158240 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 441.460407 414.547382 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 478.981439 412.668213 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 145.631843 464.253656 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 182.301119 463.047304 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 219.321147 461.706810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 256.354593 459.919086 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 293.679498 458.390795 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 331.041529 456.607362 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 368.710751 454.888225 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 405.896595 453.011349 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 442.817073 450.716802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 479.757377 449.819126 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 147.718792 501.273212 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 184.027577 499.713723 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 220.900243 498.356042 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 257.873272 497.170507 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 295.134795 495.341860 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 332.818798 493.811047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 369.922383 491.764892 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 407.220859 489.590491 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 444.497799 488.165933 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 481.216783 485.552448 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.800748 301.396632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.583031 302.657895 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 495.051988 303.072630 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 535.048017 304.115344 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 575.454313 304.746326 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 613.789947 305.827457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.443427 306.021583 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 690.851639 307.014754 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 729.463497 307.432371 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 766.408882 308.185299 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.595433 343.239121 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.825874 343.396503 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.576538 343.998569 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 535.632113 344.129014 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.903160 344.753313 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.779108 344.976061 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 652.812076 345.830393 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 691.897577 345.943833 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 729.185382 346.501661 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 766.930957 346.766697 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.909707 384.316027 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.132634 384.531966 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 495.042381 384.570784 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.969200 384.769498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 575.348749 384.842764 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 613.983574 385.239159 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.641677 385.052538 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 691.154219 385.585062 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.003780 385.453564 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.018018 385.844273 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.010062 426.056540 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.153409 425.659091 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.384977 425.570636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 535.269091 425.203636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.915834 425.176749 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.727144 424.801559 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.148022 424.989768 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.185127 424.858650 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 729.808388 425.022204 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.392857 424.561077 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 410.370000 468.036000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 452.776954 467.124135 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.386167 466.576369 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.783558 466.252206 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 575.425049 465.574951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.300525 465.456037 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.873810 464.750000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 691.720879 464.742857 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.106539 464.010974 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.471826 463.918858 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 410.162266 509.790206 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 452.297000 509.103000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.032764 507.914530 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.922249 507.287081 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.944229 506.323957 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.760278 505.480076 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.406932 505.156611 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.375439 504.332018 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.204647 504.103664 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.988121 503.086393 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 409.801835 552.064220 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 452.053398 550.733549 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 493.764505 549.738908 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.558301 548.359341 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.851852 547.304233 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.580116 546.459545 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 654.113234 545.433364 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.463529 544.602941 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.669715 543.486283 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 768.054908 543.043261 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 409.478092 594.433167 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 451.582596 592.980826 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 493.526116 591.370370 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.566132 590.002004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.891959 588.595052 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.929657 587.306134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.960275 586.077979 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.871582 584.929992 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 731.075446 583.698217 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 768.703422 582.361217 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 408.962818 636.808219 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 451.499476 635.055031 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 493.020808 633.127295 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.185048 631.754038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.857252 630.154259 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.761752 628.394231 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 654.341050 626.902880 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.788504 625.551953 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 731.352644 623.833103 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 768.964461 622.504902 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 408.995178 679.052555 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 451.286104 677.290645 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 492.915254 675.373380 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 533.995570 673.186047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.802809 671.578652 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.810612 669.645714 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 654.205402 667.791457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 693.244163 666.023349 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 731.477035 664.050104 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 769.406926 662.199134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 186.220582 281.585758 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 219.935150 257.582707 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 253.362547 234.154307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 286.876701 211.038265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 319.688632 187.981503 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 352.825800 165.146893 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 384.651256 142.795413 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 417.127890 120.274566 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 448.347710 98.286192 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 480.260181 75.996606 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 211.813258 312.072757 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 244.895688 288.745380 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 278.215420 265.382086 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 310.695108 242.437239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 343.643882 219.373840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 375.395233 197.239656 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 407.326188 175.020278 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 438.525492 152.821263 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 469.922828 130.802482 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 500.345295 109.262907 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 236.690551 342.480315 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 269.579035 318.952579 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 301.964582 295.793179 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 334.486111 273.103395 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 366.139966 250.466552 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 397.954420 228.388582 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 428.803443 206.048513 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 459.926161 184.484716 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 490.175280 162.632820 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 520.593670 140.874251 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 260.952014 371.131105 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 293.146361 348.369522 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 325.284036 325.344160 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 356.737814 302.803545 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 388.237856 280.750419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 419.185863 258.338277 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 450.063086 236.651748 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 480.050791 214.821815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 510.325690 193.570701 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 539.922832 171.944425 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 284.414474 399.833333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 316.439732 376.622024 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 347.657529 354.017649 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 378.925014 331.712367 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 409.733793 309.506107 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 440.065753 287.905023 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 470.291572 265.828018 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 500.146760 244.923888 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 529.612558 223.148965 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 558.599749 202.376412 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 307.180233 427.287468 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 338.515786 404.342878 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 369.590029 381.874637 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 400.055316 359.625704 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 430.528662 337.732059 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 460.262030 315.826632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 489.934764 295.127039 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 519.170066 272.964051 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 548.247477 252.061430 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 576.702519 231.181076 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 329.371294 453.698824 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 360.036415 431.341270 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 390.646211 408.799128 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 420.773720 386.882154 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 450.480769 364.714497 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 479.875985 343.931149 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 508.909873 321.836601 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 537.948540 301.055839 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 566.113097 279.960555 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 594.441289 259.702226 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 350.837404 479.840617 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 381.096464 457.000000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 411.059259 435.180392 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 440.655052 412.971690 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 469.980579 391.689630 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 498.949139 369.953144 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 527.561286 349.113387 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 555.651342 327.919456 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 583.816299 307.429192 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 611.377934 286.735748 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 371.970353 504.484036 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 401.488326 482.743666 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 431.002443 460.181729 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 460.004195 438.922819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 488.910013 417.018527 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 517.164992 396.238693 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 545.577342 374.815904 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 573.137751 354.455422 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 600.740126 333.535253 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 627.956121 313.945151 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 392.132898 529.083878 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 421.482955 506.781250 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 450.174537 485.351464 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 478.786033 463.397474 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 507.102445 442.511641 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 535.377049 421.083789 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 562.723848 400.581567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 590.160636 379.296223 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 617.177298 359.641386 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 643.892129 338.873352 0 diff --git a/wpical/src/main/native/resources/testcalibration.avi b/wpical/src/main/native/resources/testcalibration.avi new file mode 100644 index 0000000000..40fe6d8b75 Binary files /dev/null and b/wpical/src/main/native/resources/testcalibration.avi differ diff --git a/wpical/src/main/native/resources/testcalibration.mp4 b/wpical/src/main/native/resources/testcalibration.mp4 new file mode 100644 index 0000000000..6e4a8573a6 Binary files /dev/null and b/wpical/src/main/native/resources/testcalibration.mp4 differ diff --git a/wpical/src/main/native/resources/wpical-128.png b/wpical/src/main/native/resources/wpical-128.png new file mode 100644 index 0000000000..136b764d96 Binary files /dev/null and b/wpical/src/main/native/resources/wpical-128.png differ diff --git a/wpical/src/main/native/resources/wpical-16.png b/wpical/src/main/native/resources/wpical-16.png new file mode 100644 index 0000000000..dcd136a3c5 Binary files /dev/null and b/wpical/src/main/native/resources/wpical-16.png differ diff --git a/wpical/src/main/native/resources/wpical-256.png b/wpical/src/main/native/resources/wpical-256.png new file mode 100644 index 0000000000..4c8cb2f89e Binary files /dev/null and b/wpical/src/main/native/resources/wpical-256.png differ diff --git a/wpical/src/main/native/resources/wpical-32.png b/wpical/src/main/native/resources/wpical-32.png new file mode 100644 index 0000000000..07a5799cea Binary files /dev/null and b/wpical/src/main/native/resources/wpical-32.png differ diff --git a/wpical/src/main/native/resources/wpical-48.png b/wpical/src/main/native/resources/wpical-48.png new file mode 100644 index 0000000000..8271198862 Binary files /dev/null and b/wpical/src/main/native/resources/wpical-48.png differ diff --git a/wpical/src/main/native/resources/wpical-512.png b/wpical/src/main/native/resources/wpical-512.png new file mode 100644 index 0000000000..aa31a11303 Binary files /dev/null and b/wpical/src/main/native/resources/wpical-512.png differ diff --git a/wpical/src/main/native/resources/wpical-64.png b/wpical/src/main/native/resources/wpical-64.png new file mode 100644 index 0000000000..5816f8f50a Binary files /dev/null and b/wpical/src/main/native/resources/wpical-64.png differ diff --git a/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h b/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h new file mode 100644 index 0000000000..924e0889af --- /dev/null +++ b/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h @@ -0,0 +1,301 @@ +// -*- mode: C; c-basic-offset: 2 -*- +// Copyright 2011 Oblong Industries +// 2017 Dima Kogan +// License: GNU LGPL . + +#pragma once + +#include +#include + +typedef void (dogleg_callback_t)(const double* p, + double* x, + cholmod_sparse* Jt, + void* cookie); +typedef void (dogleg_callback_dense_t)(const double* p, + double* x, + double* J, + void* cookie); + +// an operating point of the solver +typedef struct +{ + // The pointers in this structure are all indices into a single "pool" array + // (see allocOperatingPoint()). I thus don't need to store the pointers at + // all, and can just index that one array directly, but that would break my + // ABI + double* p; + double* x; + double norm2_x; + union + { + cholmod_sparse* Jt; + double* J_dense; // row-first: grad0, grad1, grad2, ... + }; + double* Jt_x; + + // the cached update vectors. It's useful to cache these so that when a step is rejected, we can + // reuse these when we retry + double* updateCauchy; + union + { + cholmod_dense* updateGN_cholmoddense; + double* updateGN_dense; + }; + double updateCauchy_lensq, updateGN_lensq; // update vector lengths + + // whether the current update vectors are correct or not + int updateCauchy_valid, updateGN_valid; + + int didStepToEdgeOfTrustRegion; + + double* step_to_here; + double step_to_here_len_sq; + +} dogleg_operatingPoint_t; + +// The newer APIs ( dogleg_...2() ) take a dogleg_parameters2_t argument for +// their settings, while the older ones use a global set of parameters specified +// with dogleg_set_...(). This global-parameters approach doesn't work if we +// have multiple users of libdogleg in the same application +typedef struct +{ + int max_iterations; + int dogleg_debug; + + // it is cheap to reject a too-large trust region, so I start with something + // "large". The solver will quickly move down to something reasonable. Only the + // user really knows if this is "large" or not, so they should change this via + // the API + double trustregion0; + + // These are probably OK to leave alone. Tweaking them can maybe result in + // slightly faster convergence + double trustregion_decrease_factor; + double trustregion_decrease_threshold; + double trustregion_increase_factor; + double trustregion_increase_threshold; + + // The termination thresholds. Documented in the header + double Jt_x_threshold; + double update_threshold; + double trustregion_threshold; +} dogleg_parameters2_t; + +// solver context. This has all the internal state of the solver +typedef struct +{ + cholmod_common common; + + union + { + dogleg_callback_t* f; + dogleg_callback_dense_t* f_dense; + }; + void* cookie; + + // between steps, beforeStep contains the operating point of the last step. + // afterStep is ONLY used while making the step. Externally, use beforeStep + // unless you really know what you're doing + dogleg_operatingPoint_t* beforeStep; + dogleg_operatingPoint_t* afterStep; + + // The result of the last JtJ factorization performed. Note that JtJ is not + // necessarily factorized at every step, so this is NOT guaranteed to contain + // the factorization of the most recent JtJ + union + { + cholmod_factor* factorization; + + // This is a factorization of JtJ, stored as a packed symmetric matrix + // returned by dpptrf('L', ...) + double* factorization_dense; + }; + + // Have I ever seen a singular JtJ? If so, I add this constant to the diagonal + // from that point on. This is a simple and fast way to deal with + // singularities. This constant starts at 0, and is increased every time a + // singular JtJ is encountered. This is suboptimal but works for me for now. + double lambda; + + // Are we using sparse math (cholmod)? + int is_sparse; + int Nstate, Nmeasurements; + + const dogleg_parameters2_t* parameters; + +} dogleg_solverContext_t; + +#ifdef __cplusplus +extern "C" { +#endif +// Fills in the given structure with the default parameter set +void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters); + +void dogleg_setMaxIterations(int n); +void dogleg_setTrustregionUpdateParameters(double downFactor, double downThreshold, + double upFactor, double upThreshold); + +// The argument is a bit-mapped integer. Should be a bit-field structure or +// enum, but for API backwards-compatibility, I keep this an integer. +// +// if(debug == 0 ): no diagnostic output +// if(debug & DOGLEG_DEBUG_VNLOG): output vnlog diagnostics to stdout +// if(debug & ~DOGLEG_DEBUG_VNLOG): output human-oriented diagnostics to stderr +#define DOGLEG_DEBUG_VNLOG (1 << 30) +void dogleg_setDebug(int debug); + + +// The following parameters reflect the scaling of the problem being solved, so +// the user is strongly encouraged to tweak these. The defaults are set +// semi-arbitrarily + +// The size of the trust region at start. It is cheap to reject a too-large +// trust region, so this should be something "large". Say 10x the length of an +// "expected" step size +void dogleg_setInitialTrustregion(double t); + +// termination thresholds. These really depend on the scaling of the input +// problem, so the user should set these appropriately +// +// Jt_x threshold: +// The function being minimized is E = norm2(x) where x is a function of the state p. +// dE/dp = 2*Jt*x where Jt is transpose(dx/dp). +// if( for every i fabs(Jt_x[i]) < JT_X_THRESHOLD ) +// { we are done; } +// +// update threshold: +// if(for every i fabs(state_update[i]) < UPDATE_THRESHOLD) +// { we are done; } +// +// trust region threshold: +// if(trustregion < TRUSTREGION_THRESHOLD) +// { we are done; } +// +// to leave a particular threshold alone, use a value <= 0 here +void dogleg_setThresholds(double Jt_x, double update, double trustregion); + + +// The main optimization function. Initial estimate of the solution passed in p, +// final optimized solution returned in p. p has Nstate variables. There are +// Nmeas measurements, the jacobian matrix has NJnnz non-zero entries. +// +// The evaluation function is given in the callback f; this function is passed +// the given cookie +// +// If we want to get the full solver state when we're done, a non-NULL +// returnContext pointer can be given. If this is done then THE USER IS +// RESPONSIBLE FOR FREEING ITS MEMORY WITH dogleg_freeContext() +double dogleg_optimize(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, void* cookie, + dogleg_solverContext_t** returnContext); +double dogleg_optimize2(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext); + +// Main optimization function if we're using dense linear algebra. The arguments +// are very similar to the sparse version: dogleg_optimize() +double dogleg_optimize_dense(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + dogleg_solverContext_t** returnContext); +double dogleg_optimize_dense2(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext); + +// Compute the cholesky decomposition of JtJ. This function is only exposed if +// you need to touch libdogleg internals via returnContext. Sometimes after +// computing an optimization you want to do stuff with the factorization of JtJ, +// and this call ensures that the factorization is available. Most people don't +// need this function. If the comment wasn't clear, you don't need this +// function. +void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx); + +// Generates a plain text table that contains gradient checks. This table can be +// easily parsed with "vnlog" tools +void dogleg_testGradient(unsigned int var, const double* p0, + unsigned int Nstate, unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, void* cookie); +void dogleg_testGradient_dense(unsigned int var, const double* p0, + unsigned int Nstate, unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie); + + +// If we want to get the full solver state when we're done optimizing, we can +// pass a non-NULL returnContext pointer to dogleg_optimize(). If we do this, +// then the user MUST call dogleg_freeContext() to deallocate the pointer when +// the USER is done +void dogleg_freeContext(dogleg_solverContext_t** ctx); + + +// Computes outlierness factors. This function is experimental, and subject to +// change. +bool dogleg_getOutliernessFactors( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, the feature size is + // stated here + int featureSize, + int Nfeatures, + int NoutlierFeatures, // how many outliers we already have + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ); + + + + +// This stuff is experimental, and subject to change. +struct dogleg_outliers_t +{ + unsigned char marked : 1; +}; +bool dogleg_markOutliers(// output, input + struct dogleg_outliers_t* markedOutliers, + double* scale, // if <0 then I recompute + + // output, input + int* Noutliers, // number of outliers before and after this call + + // input + double (getConfidence)(int i_feature_exclude), + + // if outliers are grouped into features, the feature size is + // stated here + int featureSize, + int Nfeatures, + + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); + +void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude), + double* scale, // if <0 then I recompute + + // if outliers are grouped into features, the feature size + // is stated here + int featureSize, + int Nfeatures, + int Noutliers, // how many outliers we already have + + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); + +double dogleg_getOutliernessTrace_newFeature_sparse(const double* JqueryFeature, + int istateActive, + int NstateActive, + int featureSize, + int NoutlierFeatures, + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.cpp b/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.cpp new file mode 100644 index 0000000000..4012c48cb4 --- /dev/null +++ b/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.cpp @@ -0,0 +1,2541 @@ +// -*- mode: C; c-basic-offset: 2 -*- +// Copyright 2011 Oblong Industries +// 2017-2018 Dima Kogan +// License: GNU LGPL . + +// Apparently I need this in MSVC to get constants +#define _USE_MATH_DEFINES + +#include +#include +#include +#include +#include +#include +#include +#include "dogleg.h" + +#if (CHOLMOD_VERSION > (CHOLMOD_VER_CODE(2,2))) && (CHOLMOD_VERSION < (CHOLMOD_VER_CODE(4,0))) +#include +#endif + + +// Any non-vnlog bit mask +#define DOGLEG_DEBUG_OTHER_THAN_VNLOG (~DOGLEG_DEBUG_VNLOG) +#define SAY_NONEWLINE(fmt, ...) fprintf(stderr, "libdogleg at %s:%d: " fmt, __FILE__, __LINE__, ## __VA_ARGS__) +#define SAY(fmt, ...) do { SAY_NONEWLINE(fmt, ## __VA_ARGS__); fprintf(stderr, "\n"); } while(0) + +// This REQUIRES that a "dogleg_solverContext_t* ctx" be available +#define SAY_IF_VERBOSE(fmt,...) do { if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_OTHER_THAN_VNLOG ) SAY(fmt, ##__VA_ARGS__); } while(0) + +// I do this myself because I want this to be active in all build modes, not just !NDEBUG +#define ASSERT(x) do { if(!(x)) { SAY("ASSERTION FAILED: " #x " is not true"); exit(1); } } while(0) + +// used to consolidate the casts +#define P(A, index) ((unsigned int*)((A)->p))[index] +#define I(A, index) ((unsigned int*)((A)->i))[index] +#define X(A, index) ((double* )((A)->x))[index] + + +////////////////////////////////////////////////////////////////////////////////////////// +// vnlog debugging stuff +// +// This is used if the user calls dogleg_setDebug(DOGLEG_DEBUG_VNLOG | stuff) +////////////////////////////////////////////////////////////////////////////////////////// +#define VNLOG_DEBUG_STEP_TYPE_LIST(_) \ + _(STEPTYPE_CAUCHY, "cauchy") \ + _(STEPTYPE_GAUSSNEWTON, "gaussnewton") \ + _(STEPTYPE_INTERPOLATED, "interpolated") \ + _(STEPTYPE_FAILED, "failed") +#define VNLOG_DEBUG_STEP_TYPE_NAME_COMMA(name,shortname) name, +typedef enum { VNLOG_DEBUG_STEP_TYPE_LIST(VNLOG_DEBUG_STEP_TYPE_NAME_COMMA) + STEPTYPE_UNINITIALIZED } vnlog_debug_step_type_t; +#define VNLOG_DEBUG_FIELDS(_) \ + _(double, norm2x_before, INFINITY) \ + _(double, norm2x_after, INFINITY) \ + _(double, step_len_cauchy, INFINITY) \ + _(double, step_len_gauss_newton, INFINITY) \ + _(double, step_len_interpolated, INFINITY) \ + _(double, k_cauchy_to_gn, INFINITY) \ + _(double, step_len, INFINITY) \ + _(vnlog_debug_step_type_t, step_type, STEPTYPE_UNINITIALIZED) \ + _(double, step_direction_change_deg, INFINITY) \ + _(double, expected_improvement, INFINITY) \ + _(double, observed_improvement, INFINITY) \ + _(double, rho, INFINITY) \ + _(double, trustregion_before, INFINITY) \ + _(double, trustregion_after, INFINITY) +static struct vnlog_debug_data_t +{ +#define VNLOG_DEBUG_DECLARE_FIELD(type, name, initialvalue) type name; + VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_DECLARE_FIELD) +} vnlog_debug_data; +static void vnlog_debug_reset(void) +{ +#define VNLOG_DEBUG_RESET_FIELD(type, name, initialvalue) vnlog_debug_data.name = initialvalue; + VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_RESET_FIELD); +} +static void vnlog_debug_emit_legend(void) +{ +#define VNLOG_DEBUG_SPACE_FIELD_NAME(type, name, initialvalue) " " #name + + vnlog_debug_reset(); + printf("# iteration step_accepted" VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_SPACE_FIELD_NAME) "\n"); +} + +static void vnlog_emit_double(double x) +{ + if(x == INFINITY) printf("- "); + else printf("%g ", x); +} + +static void vnlog_emit_int(int x) +{ + if(x == -1) printf("- "); + else printf("%d ", x); +} + +static void vnlog_emit_vnlog_debug_step_type_t(vnlog_debug_step_type_t x) +{ +#define VNLOG_DEBUG_STEP_TYPE_SWITCH_EMIT(name,shortname) case name: printf(shortname " "); break; + switch(x) + { + VNLOG_DEBUG_STEP_TYPE_LIST(VNLOG_DEBUG_STEP_TYPE_SWITCH_EMIT) + default: printf("- "); + } +} +static void vnlog_debug_emit_record(int iteration, int step_accepted) +{ +#define VNLOG_DEBUG_EMIT_FIELD(type, name, initialvalue) vnlog_emit_ ## type(vnlog_debug_data.name); + + printf("%d %d ", iteration, step_accepted); + VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_EMIT_FIELD); + printf("\n"); + fflush(stdout); + vnlog_debug_reset(); +} + +// Default parameters. Used only by the original API, which uses a global set of +// parameters +#define PARAMETERS_DEFAULT \ + { \ + .max_iterations = 100, \ + .dogleg_debug = 0, \ + .trustregion0 = 1.0e3, \ + .trustregion_decrease_factor = 0.1, \ + .trustregion_decrease_threshold = 0.25, \ + .trustregion_increase_factor = 2, \ + .trustregion_increase_threshold = 0.75, \ + .Jt_x_threshold = 1e-8, \ + .update_threshold = 1e-8, \ + .trustregion_threshold = 1e-8 \ + } + +static const dogleg_parameters2_t parameters_default = PARAMETERS_DEFAULT; +static dogleg_parameters2_t parameters_global = PARAMETERS_DEFAULT; +void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters) +{ + *parameters = parameters_default; +} + +// if I ever see a singular JtJ, I factor JtJ + LAMBDA*I from that point on +#define LAMBDA_INITIAL 1e-10 + +// these parameters likely should be messed with +void dogleg_setDebug(int debug) +{ + parameters_global.dogleg_debug = debug; +} +void dogleg_setInitialTrustregion(double t) +{ + parameters_global.trustregion0 = t; +} +void dogleg_setThresholds(double Jt_x, double update, double trustregion) +{ + if(Jt_x > 0.0) parameters_global.Jt_x_threshold = Jt_x; + if(update > 0.0) parameters_global.update_threshold = update; + if(trustregion > 0.0) parameters_global.trustregion_threshold = trustregion; +} + +// these parameters likely should not be messed with. +void dogleg_setMaxIterations(int n) +{ + parameters_global.max_iterations = n; +} +void dogleg_setTrustregionUpdateParameters(double downFactor, double downThreshold, + double upFactor, double upThreshold) +{ + parameters_global.trustregion_decrease_factor = downFactor; + parameters_global.trustregion_decrease_threshold = downThreshold; + parameters_global.trustregion_increase_factor = upFactor; + parameters_global.trustregion_increase_threshold = upThreshold; +} + + + + +////////////////////////////////////////////////////////////////////////////////////////// +// general boring linear algebra stuff +// should really come from BLAS or libminimath +////////////////////////////////////////////////////////////////////////////////////////// +static double norm2(const double* x, unsigned int n) +{ + double result = 0; + for(unsigned int i=0; inrow); + for(unsigned int i=0; incol; i++) + { + for(unsigned int j=P(A, i); jncol; i++) + { + double dotproduct = 0.0; + for(unsigned int j=P(At, i); j 0; + + double* x0 = static_cast(malloc(Nmeas * sizeof(double))); + double* x = static_cast(malloc(Nmeas * sizeof(double))); + double* p = static_cast(malloc(Nstate * sizeof(double))); + ASSERT(x0); + ASSERT(x); + ASSERT(p); + + memcpy(p, p0, Nstate * sizeof(double)); + + + cholmod_common _cholmod_common; + cholmod_sparse* Jt; + cholmod_sparse* Jt0; + double* J_dense = NULL; // setting to NULL to pacify compiler's "uninitialized" warnings + double* J_dense0 = NULL; // setting to NULL to pacify compiler's "uninitialized" warnings + + + // This is a plain text table, that can be easily parsed with "vnlog" tools + printf("# ivar imeasurement gradient_reported gradient_observed error error_relative\n"); + + if( is_sparse ) + { + if( !cholmod_start(&_cholmod_common) ) + { + SAY( "Couldn't initialize CHOLMOD"); + return; + } + + Jt = cholmod_allocate_sparse(Nstate, Nmeas, NJnnz, + 1, // sorted + 1, // packed, + 0, // NOT symmetric + CHOLMOD_REAL, + &_cholmod_common); + Jt0 = cholmod_allocate_sparse(Nstate, Nmeas, NJnnz, + 1, // sorted + 1, // packed, + 0, // NOT symmetric + CHOLMOD_REAL, + &_cholmod_common); + + p[var] -= GRADTEST_DELTA/2.0; + (*f)(p, x0, Jt0, cookie); + p[var] += GRADTEST_DELTA; + (*f)(p, x, Jt, cookie); + p[var] -= GRADTEST_DELTA/2.0; + } + else + { + J_dense = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); + J_dense0 = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); + + dogleg_callback_dense_t* f_dense = (dogleg_callback_dense_t*)f; + p[var] -= GRADTEST_DELTA/2.0; + (*f_dense)(p, x0, J_dense0, cookie); + p[var] += GRADTEST_DELTA; + (*f_dense)(p, x, J_dense, cookie); + p[var] -= GRADTEST_DELTA/2.0; + } + + + for(unsigned int i=0; i 0, instead I have %d", NJnnz); + return; + } + return _dogleg_testGradient(var, p0, Nstate, Nmeas, NJnnz, f, cookie); +} +void dogleg_testGradient_dense(unsigned int var, const double* p0, + unsigned int Nstate, unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie) +{ + return _dogleg_testGradient(var, p0, Nstate, Nmeas, 0, (dogleg_callback_t*)f, cookie); +} + + +////////////////////////////////////////////////////////////////////////////////////////// +// solver routines +////////////////////////////////////////////////////////////////////////////////////////// + +static void computeCauchyUpdate(dogleg_operatingPoint_t* point, + const dogleg_solverContext_t* ctx) +{ + // I already have this data, so don't need to recompute + if(!point->updateCauchy_valid) + { + point->updateCauchy_valid = 1; + + // I look at a step in the steepest direction that minimizes my + // quadratic error function (Cauchy point). If this is past my trust region, + // I move as far as the trust region allows along the steepest descent + // direction. My error function is F=norm2(f(p)). dF/dP = 2*ft*J. + // This is proportional to Jt_x, which is thus the steepest ascent direction. + // + // Thus along this direction we have F(k) = norm2(f(p + k Jt_x)). The Cauchy + // point is where F(k) is at a minimum: + // dF_dk = 2 f(p + k Jt_x)t J Jt_x ~ (x + k J Jt_x)t J Jt_x = + // = xt J Jt x + k xt J Jt J Jt x = norm2(Jt x) + k norm2(J Jt x) + // dF_dk = 0 -> k= -norm2(Jt x) / norm2(J Jt x) + // Summary: + // the steepest direction is parallel to Jt*x. The Cauchy point is at + // k*Jt*x where k = -norm2(Jt*x)/norm2(J*Jt*x) + double norm2_Jt_x = norm2(point->Jt_x, ctx->Nstate); + double norm2_J_Jt_x = ctx->is_sparse ? + norm2_mul_spmatrix_t_densevector(point->Jt, point->Jt_x) : + norm2_mul_matrix_vector (point->J_dense, point->Jt_x, ctx->Nmeasurements, ctx->Nstate); + double k = -norm2_Jt_x / norm2_J_Jt_x; + + point->updateCauchy_lensq = k*k * norm2_Jt_x; + + vec_copy_scaled(point->updateCauchy, + point->Jt_x, k, ctx->Nstate); + SAY_IF_VERBOSE( "cauchy step size %.6g", sqrt(point->updateCauchy_lensq)); + } + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_data.step_len_cauchy = sqrt(point->updateCauchy_lensq); +} + +// LAPACK prototypes for a packed cholesky factorization and a linear solve +// using that factorization, respectively +extern "C" { +int dpptrf_(char* uplo, int* n, double* ap, + int* info, int uplo_len); +int dpptrs_(char* uplo, int* n, int* nrhs, + double* ap, double* b, int* ldb, int* info, + int uplo_len); +} + + +void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +{ + // I already have this data, so don't need to recompute + if(point->updateGN_valid) + return; + + if( ctx->is_sparse ) + { + // I'm assuming the pattern of zeros will remain the same throughout, so I + // analyze only once + if(ctx->factorization == NULL) + { + ctx->factorization = cholmod_analyze(point->Jt, &ctx->common); + ASSERT(ctx->factorization != NULL); + } + + while(1) + { + if( ctx->lambda == 0.0 ) + ASSERT( cholmod_factorize(point->Jt, ctx->factorization, &ctx->common) ); + else + { + double beta[] = { ctx->lambda, 0 }; + ASSERT( cholmod_factorize_p(point->Jt, beta, NULL, 0, + ctx->factorization, &ctx->common) ); + } + + if(ctx->factorization->minor == ctx->factorization->n) + break; + + // singular JtJ. Raise lambda and go again + if( ctx->lambda == 0.0) ctx->lambda = LAMBDA_INITIAL; + else ctx->lambda *= 10.0; + ASSERT( isfinite(ctx->lambda) ); + + SAY_IF_VERBOSE( "singular JtJ. Have rank/full rank: %zd/%d. Adding %g I from now on", + ctx->factorization->minor, ctx->Nstate, ctx->lambda); + } + } + else + { + if(ctx->factorization_dense == NULL) + { + // Need to store symmetric JtJ, so I only need one triangle of it + ctx->factorization_dense = static_cast(malloc( ctx->Nstate * (ctx->Nstate+1) / 2 * + sizeof( ctx->factorization_dense[0]))); + ASSERT(ctx->factorization_dense); + } + + while(1) + { + // I construct my JtJ. JtJ is packed and stored row-first. I have two + // equivalent implementations. The one enabled here is maybe a bit faster, + // but it's definitely clearer +#if 1 + memset(ctx->factorization_dense, + 0, + ctx->Nstate*(ctx->Nstate+1)/2*sizeof(ctx->factorization_dense[0])); + for(int i=0; iNmeasurements; i++) + accum_outerproduct_packed( ctx->factorization_dense, &point->J_dense[ctx->Nstate*i], + ctx->Nstate ); + if( ctx->lambda > 0.0 ) + { + int iJtJ=0; + for(int i1=0; i1Nstate; i1++) + { + ctx->factorization_dense[iJtJ] += ctx->lambda; + iJtJ += ctx->Nstate-i1; + } + } +#else + int iJtJ = 0; + for(int i1=0; i1Nstate; i1++) + { + #error this does not work. overwritten in the following loop + ctx->factorization_dense[iJtJ] += ctx->lambda; + + for(int i0=i1; i0Nstate; i0++, iJtJ++) + ctx->factorization_dense[iJtJ] = inner_withstride( &point->J_dense[i0], + &point->J_dense[i1], + ctx->Nmeasurements, + ctx->Nstate); + } +#endif + + + + int info; + char uplo = 'L'; + int Nstate_copy = ctx->Nstate; + dpptrf_(&uplo, &Nstate_copy, ctx->factorization_dense, + &info, 1); + ASSERT(info >= 0); // we MUST either succeed or see complain of singular + // JtJ + if( info == 0 ) + break; + + // singular JtJ. Raise lambda and go again + if( ctx->lambda == 0.0) ctx->lambda = LAMBDA_INITIAL; + else ctx->lambda *= 10.0; + + SAY_IF_VERBOSE( "singular JtJ. Adding %g I from now on", ctx->lambda); + } + } +} + +static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +{ + // I already have this data, so don't need to recompute + if(!point->updateGN_valid) + { + dogleg_computeJtJfactorization(point, ctx); + + // try to factorize the matrix directly. If it's singular, add a small + // constant to the diagonal. This constant gets larger if we keep being + // singular + if( ctx->is_sparse ) + { + // solve JtJ*updateGN = Jt*x. Gauss-Newton step is then -updateGN + cholmod_dense Jt_x_dense = {.nrow = static_cast(ctx->Nstate), + .ncol = 1, + .nzmax = static_cast(ctx->Nstate), + .d = static_cast(ctx->Nstate), + .x = point->Jt_x, + .xtype = CHOLMOD_REAL, + .dtype = CHOLMOD_DOUBLE}; + + if(point->updateGN_cholmoddense != NULL) + cholmod_free_dense(&point->updateGN_cholmoddense, &ctx->common); + + point->updateGN_cholmoddense = cholmod_solve(CHOLMOD_A, + ctx->factorization, + &Jt_x_dense, + &ctx->common); + vec_negate(static_cast(point->updateGN_cholmoddense->x), + ctx->Nstate); // should be more efficient than this later + + point->updateGN_lensq = norm2(static_cast(point->updateGN_cholmoddense->x), ctx->Nstate); + } + else + { + memcpy( point->updateGN_dense, point->Jt_x, ctx->Nstate * sizeof(point->updateGN_dense[0])); + int info; + char uplo = 'L'; + int nhrs = 1; + int Nstate_copy = ctx->Nstate; + int Nstate_copy2 = ctx->Nstate; + dpptrs_(&uplo, &Nstate_copy, &nhrs, + ctx->factorization_dense, + point->updateGN_dense, &Nstate_copy2, &info, 1); + vec_negate(point->updateGN_dense, + ctx->Nstate); // should be more efficient than this later + + point->updateGN_lensq = norm2(point->updateGN_dense, ctx->Nstate); + } + + SAY_IF_VERBOSE( "gn step size %.6g", sqrt(point->updateGN_lensq)); + point->updateGN_valid = 1; + } + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_data.step_len_gauss_newton = sqrt(point->updateGN_lensq); + +} + +static void computeInterpolatedUpdate(double* update_dogleg, + double* update_dogleg_lensq, + dogleg_operatingPoint_t* point, + double trustregion, + const dogleg_solverContext_t* ctx) +{ + // I interpolate between the Cauchy-point step and the Gauss-Newton step + // to find a step that takes me to the edge of my trust region. + // + // I have something like norm2(a + k*(b-a)) = dsq + // = norm2(a) + 2*at*(b-a) * k + norm2(b-a)*k^2 = dsq + // let c = at*(b-a), l2 = norm2(b-a) -> + // l2 k^2 + 2*c k + norm2(a)-dsq = 0 + // + // This is a simple quadratic equation: + // k = (-2*c +- sqrt(4*c*c - 4*l2*(norm2(a)-dsq)))/(2*l2) + // = (-c +- sqrt(c*c - l2*(norm2(a)-dsq)))/l2 + + // to make 100% sure the discriminant is positive, I choose a to be the + // cauchy step. The solution must have k in [0,1], so I much have the + // +sqrt side, since the other one is negative + double dsq = trustregion*trustregion; + double norm2a = point->updateCauchy_lensq; + const double* a = point->updateCauchy; + const double* b = ctx->is_sparse ? static_cast(point->updateGN_cholmoddense->x) : point->updateGN_dense; + + double l2 = 0.0; + double neg_c = 0.0; + for(int i=0; iNstate; i++) + { + double d = a[i] - b[i]; + + l2 += d*d; + neg_c += d*a[i]; + } + + double discriminant = neg_c*neg_c - l2* (norm2a - dsq); + if(discriminant < 0.0) + { + SAY( "negative discriminant: %.6g!", discriminant); + discriminant = 0.0; + } + double k = (neg_c + sqrt(discriminant))/l2; + + *update_dogleg_lensq = 0.0; + for(int i=0; iNstate; i++) + { + update_dogleg[i] = a[i] + k*(b[i] - a[i]); + *update_dogleg_lensq += update_dogleg[i]*update_dogleg[i]; + } + + SAY_IF_VERBOSE( "k_cauchy_to_gn %.6g, norm %.6g", + k, + sqrt(*update_dogleg_lensq)); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_len_interpolated = sqrt(*update_dogleg_lensq); + vnlog_debug_data.k_cauchy_to_gn = k; + } +} + +// takes in point->p, and computes all the quantities derived from it, storing +// the result in the other members of the operatingPoint structure. Returns +// true if the gradient-size termination criterion has been met +static int computeCallbackOperatingPoint(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +{ + if( ctx->is_sparse ) + { + (*ctx->f)(point->p, point->x, point->Jt, ctx->cookie); + + // compute Jt*x + mul_spmatrix_densevector(point->Jt_x, point->Jt, point->x); + } + else + { + (*ctx->f_dense)(point->p, point->x, point->J_dense, ctx->cookie); + + // compute Jt*x + mul_matrix_t_densevector(point->Jt_x, point->J_dense, point->x, + ctx->Nmeasurements, ctx->Nstate); + } + + // I just got a new operating point, so the current update vectors aren't + // valid anymore, and should be recomputed, as needed + point->updateCauchy_valid = 0; + point->updateGN_valid = 0; + + // compute the 2-norm of the current error vector + // At some point this should be changed to use the call from libminimath + point->norm2_x = norm2(point->x, ctx->Nmeasurements); + + // If the largest absolute gradient element is smaller than the threshold, + // we can stop iterating. This is equivalent to the inf-norm + for(int i=0; iNstate; i++) + if(fabs(point->Jt_x[i]) > ctx->parameters->Jt_x_threshold) + return 0; + SAY_IF_VERBOSE( "Jt_x all below the threshold. Done iterating!"); + + return 1; +} +static double computeExpectedImprovement(const double* step, const dogleg_operatingPoint_t* point, + const dogleg_solverContext_t* ctx) +{ + // My error function is F=norm2(f(p + step)). F(0) - F(step) = + // = norm2(x) - norm2(x + J*step) = -2*inner(x,J*step) - norm2(J*step) + // = -2*inner(Jt_x,step) - norm2(J*step) + if( ctx->is_sparse ) + return + - 2.0*inner(point->Jt_x, step, ctx->Nstate) + - norm2_mul_spmatrix_t_densevector(point->Jt, step); + else + return + - 2.0*inner(point->Jt_x, step, ctx->Nstate) + - norm2_mul_matrix_vector(point->J_dense, step, ctx->Nmeasurements, ctx->Nstate); +} + + +// takes a step from the given operating point, using the given trust region +// radius. Returns the expected improvement, based on the step taken and the +// linearized x(p). If we can stop iterating, returns a negative number +static double takeStepFrom(dogleg_operatingPoint_t* pointFrom, + double* p_new, + double* step, + double* step_len_sq, + double trustregion, + dogleg_solverContext_t* ctx) +{ + SAY_IF_VERBOSE( "taking step with trustregion %.6g", trustregion); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.trustregion_before = trustregion; + vnlog_debug_data.norm2x_before = pointFrom->norm2_x; + } + + computeCauchyUpdate(pointFrom, ctx); + + if(pointFrom->updateCauchy_lensq >= trustregion*trustregion) + { + SAY_IF_VERBOSE( "taking cauchy step"); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_type = STEPTYPE_CAUCHY; + vnlog_debug_data.step_len = vnlog_debug_data.step_len_cauchy; + } + *step_len_sq = pointFrom->updateCauchy_lensq; + + // cauchy step goes beyond my trust region, so I do a gradient descent + // to the edge of my trust region and call it good + vec_copy_scaled(step, + pointFrom->updateCauchy, + trustregion / sqrt(pointFrom->updateCauchy_lensq), + ctx->Nstate); + pointFrom->didStepToEdgeOfTrustRegion = 1; + } + else + { + // I'm not yet done. The cauchy point is within the trust region, so I can + // go further. I look at the full Gauss-Newton step. If this is within the + // trust region, I use it. Otherwise, I find the point at the edge of my + // trust region that lies on a straight line between the Cauchy point and + // the Gauss-Newton solution, and use that. This is the heart of Powell's + // dog-leg algorithm. + computeGaussNewtonUpdate(pointFrom, ctx); + if(pointFrom->updateGN_lensq <= trustregion*trustregion) + { + SAY_IF_VERBOSE( "taking GN step"); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_type = STEPTYPE_GAUSSNEWTON; + vnlog_debug_data.step_len = vnlog_debug_data.step_len_gauss_newton; + } + *step_len_sq = pointFrom->updateGN_lensq; + + // full Gauss-Newton step lies within my trust region. Take the full step + memcpy( step, + ctx->is_sparse ? pointFrom->updateGN_cholmoddense->x : pointFrom->updateGN_dense, + ctx->Nstate * sizeof(step[0]) ); + pointFrom->didStepToEdgeOfTrustRegion = 0; + } + else + { + SAY_IF_VERBOSE( "taking interpolated step"); + + // full Gauss-Newton step lies outside my trust region, so I interpolate + // between the Cauchy-point step and the Gauss-Newton step to find a step + // that takes me to the edge of my trust region. + computeInterpolatedUpdate(step, + step_len_sq, + pointFrom, trustregion, ctx); + pointFrom->didStepToEdgeOfTrustRegion = 1; + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_type = STEPTYPE_INTERPOLATED; + vnlog_debug_data.step_len = vnlog_debug_data.step_len_interpolated; + } + } + } + + // take the step + vec_add(p_new, pointFrom->p, step, ctx->Nstate); + double expectedImprovement = computeExpectedImprovement(step, pointFrom, ctx); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.expected_improvement = expectedImprovement; + + if(pointFrom->step_to_here_len_sq != INFINITY) + { + double cos_direction_change = + inner(step, pointFrom->step_to_here, ctx->Nstate) / + sqrt(*step_len_sq * pointFrom->step_to_here_len_sq); + + // check the numerical overflow cases + if(cos_direction_change >= 1.0) + vnlog_debug_data.step_direction_change_deg = 0.0; + else if(cos_direction_change <= -1.0) + vnlog_debug_data.step_direction_change_deg = 180.0; + else + vnlog_debug_data.step_direction_change_deg = 180.0/M_PI*acos(cos_direction_change); + } + } + + // are we done? For each state variable I look at the update step. If all the elements fall below + // a threshold, I call myself done + for(int i=0; iNstate; i++) + if( fabs(step[i]) > ctx->parameters->update_threshold ) + return expectedImprovement; + + SAY_IF_VERBOSE( "update small enough. Done iterating!"); + + return -1.0; +} + + +// I have a candidate step. I adjust the trustregion accordingly, and also +// report whether this step should be accepted (0 == rejected, otherwise +// accepted) +static int evaluateStep_adjustTrustRegion(const dogleg_operatingPoint_t* before, + const dogleg_operatingPoint_t* after, + double* trustregion, + double expectedImprovement, + dogleg_solverContext_t* ctx) +{ + double observedImprovement = before->norm2_x - after->norm2_x; + double rho = observedImprovement / expectedImprovement; + SAY_IF_VERBOSE( "observed/expected improvement: %.6g/%.6g. rho = %.6g", + observedImprovement, expectedImprovement, rho); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.observed_improvement = observedImprovement; + vnlog_debug_data.rho = rho; + } + + + // adjust the trust region + if( rho < ctx->parameters->trustregion_decrease_threshold ) + { + SAY_IF_VERBOSE( "rho too small. decreasing trust region"); + + // Our model doesn't fit well. We should reduce the trust region size. If + // the trust region size was affecting the attempted step, do this by a + // constant factor. Otherwise, drop the trustregion to attempted step size + // first + if( !before->didStepToEdgeOfTrustRegion ) + *trustregion = sqrt(before->updateGN_lensq); + + *trustregion *= ctx->parameters->trustregion_decrease_factor; + } + else if (rho > ctx->parameters->trustregion_increase_threshold && before->didStepToEdgeOfTrustRegion) + { + SAY_IF_VERBOSE( "rho large enough. increasing trust region"); + + *trustregion *= ctx->parameters->trustregion_increase_factor; + } + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + vnlog_debug_data.trustregion_after = *trustregion; + + return rho > 0.0; +} + +static int runOptimizer(dogleg_solverContext_t* ctx) +{ + double trustregion = ctx->parameters->trustregion0; + int stepCount = 0; + + if( computeCallbackOperatingPoint(ctx->beforeStep, ctx) ) + return stepCount; + + SAY_IF_VERBOSE( "Initial operating point has norm2_x %.6g", ctx->beforeStep->norm2_x); + + + ctx->beforeStep->step_to_here_len_sq = INFINITY; + + while( stepCountparameters->max_iterations ) + { + SAY_IF_VERBOSE( "================= step %d", stepCount ); + + while(1) + { + SAY_IF_VERBOSE("--------"); + + double expectedImprovement = + takeStepFrom(ctx->beforeStep, + ctx->afterStep->p, + ctx->afterStep->step_to_here, + &ctx->afterStep->step_to_here_len_sq, + trustregion, ctx); + + // negative expectedImprovement is used to indicate that we're done + if(expectedImprovement < 0.0) + { + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_record(stepCount, 1); + return stepCount; + } + + int afterStepZeroGradient = computeCallbackOperatingPoint(ctx->afterStep, ctx); + SAY_IF_VERBOSE( "Evaluated operating point with norm2_x %.6g", ctx->afterStep->norm2_x); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + vnlog_debug_data.norm2x_after = ctx->afterStep->norm2_x; + + if( evaluateStep_adjustTrustRegion(ctx->beforeStep, ctx->afterStep, &trustregion, + expectedImprovement, ctx) ) + { + SAY_IF_VERBOSE( "accepted step"); + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_record(stepCount, 1); + stepCount++; + + // I accept this step, so the after-step operating point is the before-step operating point + // of the next iteration. I exchange the before- and after-step structures so that all the + // pointers are still around and I don't have to re-allocate + dogleg_operatingPoint_t* tmp; + tmp = ctx->afterStep; + ctx->afterStep = ctx->beforeStep; + ctx->beforeStep = tmp; + + if( afterStepZeroGradient ) + { + SAY_IF_VERBOSE( "Gradient low enough and we just improved. Done iterating!" ); + + return stepCount; + } + + break; + } + + SAY_IF_VERBOSE( "rejected step"); + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_record(stepCount, 0); + + // This step was rejected. check if the new trust region size is small + // enough to give up + if(trustregion < ctx->parameters->trustregion_threshold) + { + SAY_IF_VERBOSE( "trust region small enough. Giving up. Done iterating!"); + return stepCount; + } + + // I have rejected this step, so I try again with the new trust region + } + } + + if(stepCount == ctx->parameters->max_iterations) + SAY_IF_VERBOSE( "Exceeded max number of iterations"); + + return stepCount; +} + +static +dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements, + unsigned int NJnnz, + cholmod_common* common) +{ + int is_sparse = NJnnz > 0; + + dogleg_operatingPoint_t* point = static_cast(malloc(sizeof(dogleg_operatingPoint_t))); + ASSERT(point != NULL); + + + int Npool = + Nstate + + numMeasurements + + Nstate + + Nstate + + Nstate; + if(!is_sparse) + Npool += numMeasurements*Nstate + Nstate; + + double* pool = static_cast(malloc( Npool * sizeof(double) )); + ASSERT( pool != NULL ); + + point->p = &pool[0]; + point->x = &pool[Nstate]; + point->Jt_x = &pool[Nstate + + numMeasurements]; + point->updateCauchy = &pool[Nstate + + numMeasurements + + Nstate]; + point->step_to_here = &pool[Nstate + + numMeasurements + + Nstate + + Nstate]; + + if( is_sparse ) + { + point->Jt = cholmod_allocate_sparse(Nstate, numMeasurements, NJnnz, + 1, // sorted + 1, // packed, + 0, // NOT symmetric + CHOLMOD_REAL, + common); + ASSERT(point->Jt != NULL); + point->updateGN_cholmoddense = NULL; // This will be allocated as it is used + } + else + { + point->J_dense = &pool[Nstate + + numMeasurements + + Nstate + + Nstate + + Nstate]; + point->updateGN_dense = &pool[Nstate + + numMeasurements + + Nstate + + Nstate + + Nstate + + numMeasurements * Nstate]; + } + + // vectors don't have any valid data yet + point->updateCauchy_valid = 0; + point->updateGN_valid = 0; + + return point; +} + +static void freeOperatingPoint(dogleg_operatingPoint_t** point, cholmod_common* common) +{ + // MUST match allocOperatingPoint(). It does a single malloc() and stores the + // pointer into p + free((*point)->p); + + int is_sparse = common != NULL; + + if( is_sparse ) + { + cholmod_free_sparse(&(*point)->Jt, common); + + if((*point)->updateGN_cholmoddense != NULL) + cholmod_free_dense(&(*point)->updateGN_cholmoddense, common); + } + + free(*point); + *point = NULL; +} + +static int cholmod_error_callback(const char* s, ...) +{ + SAY_NONEWLINE(""); + + va_list ap; + va_start(ap, s); + int ret = vfprintf(stderr, s, ap); + va_end(ap); + fprintf(stderr, "\n"); + return ret; +} + +static void set_cholmod_options(cholmod_common* cc) +{ + // I want to use LGPL parts of CHOLMOD only, so I turn off the supernodal routines. This gave me a + // 25% performance hit in the solver for a particular set of optical calibration data. + cc->supernodal = 0; + + + // I want all output to go to STDERR, not STDOUT +#if (CHOLMOD_VERSION <= (CHOLMOD_VER_CODE(2,2))) + cc->print_function = cholmod_error_callback; +#elif (CHOLMOD_VERSION < (CHOLMOD_VER_CODE(4,0))) + CHOLMOD_FUNCTION_DEFAULTS ; + CHOLMOD_FUNCTION_PRINTF(cc) = cholmod_error_callback; +#else + SuiteSparse_config_printf_func_set(cholmod_error_callback); +#endif +} + +void dogleg_freeContext(dogleg_solverContext_t** ctx) +{ + freeOperatingPoint(&(*ctx)->beforeStep, (*ctx)->is_sparse ? &(*ctx)->common : NULL); + freeOperatingPoint(&(*ctx)->afterStep, (*ctx)->is_sparse ? &(*ctx)->common : NULL); + + if( (*ctx)->is_sparse ) + { + if((*ctx)->factorization != NULL) + cholmod_free_factor (&(*ctx)->factorization, &(*ctx)->common); + cholmod_finish(&(*ctx)->common); + } + else + free((*ctx)->factorization_dense); + + free(*ctx); + *ctx = NULL; +} + +static double _dogleg_optimize(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, + void* cookie, + + // NULL to use the globals + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext) +{ + int is_sparse = NJnnz > 0; + + + dogleg_solverContext_t* ctx = static_cast(malloc(sizeof(dogleg_solverContext_t))); + ctx->f = f; + ctx->cookie = cookie; + ctx->factorization = NULL; + ctx->lambda = 0.0; + ctx->Nstate = Nstate; + ctx->Nmeasurements = Nmeas; + ctx->parameters = parameters ? parameters : ¶meters_global; + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_legend(); + + if( returnContext != NULL ) + *returnContext = ctx; + + if( is_sparse ) + { + if( !cholmod_start(&ctx->common) ) + { + SAY( "Couldn't initialize CHOLMOD"); + return -1.0; + } + set_cholmod_options(&ctx->common); + ctx->is_sparse = 1; + } + else + ctx->is_sparse = 0; + + ctx->beforeStep = allocOperatingPoint(Nstate, Nmeas, NJnnz, &ctx->common); + ctx->afterStep = allocOperatingPoint(Nstate, Nmeas, NJnnz, &ctx->common); + + memcpy(ctx->beforeStep->p, p, Nstate * sizeof(double)); + + // everything is set up, so run the solver! + int numsteps = runOptimizer(ctx); + double norm2_x = ctx->beforeStep->norm2_x; + + // runOptimizer places the most recent results into beforeStep in preparation for another + // iteration + memcpy(p, ctx->beforeStep->p, Nstate * sizeof(double)); + + SAY_IF_VERBOSE( "success! took %d iterations", numsteps); + + if( returnContext == NULL ) + dogleg_freeContext(&ctx); + + return norm2_x; +} + +double dogleg_optimize2(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, + void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext) +{ + if( NJnnz == 0 ) + { + SAY( "I must have NJnnz > 0, instead I have %d", NJnnz); + return -1.0; + } + + return _dogleg_optimize(p, Nstate, Nmeas, NJnnz, f, + cookie, + parameters, + returnContext); +} + +double dogleg_optimize(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, + void* cookie, + dogleg_solverContext_t** returnContext) +{ + return dogleg_optimize2(p,Nstate,Nmeas,NJnnz,f,cookie, + NULL, // no parameters; use the globals + returnContext); +} + + +double dogleg_optimize_dense2(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext) +{ + return _dogleg_optimize(p, Nstate, Nmeas, 0, (dogleg_callback_t*)f, + cookie, + parameters, + returnContext); +} +double dogleg_optimize_dense(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + dogleg_solverContext_t** returnContext) +{ + return dogleg_optimize_dense2(p,Nstate,Nmeas,f,cookie, + NULL, // no parameters; use the globals + returnContext); +} + + + + + + + +// Computes pinv(J) for a subset of measurements: inv(JtJ) * +// Jt[imeasurement0..imeasurement0+N-1]. Returns false if something failed. +// ASSUMES THAT THE CHOLESKY FACTORIZATION HAS ALREADY BEEN COMPUTED. +// +// This function is experimental, and subject to change +static bool pseudoinverse_J_dense(// output + double* out, + + // inputs + const dogleg_operatingPoint_t* point, + const dogleg_solverContext_t* ctx, + int i_meas0, int NmeasInChunk) +{ + int info; + memcpy(out, + &point->J_dense[i_meas0*ctx->Nstate], + NmeasInChunk*ctx->Nstate*sizeof(double)); + char uplo = 'L'; + int Nstate_copy = ctx->Nstate; + int Nstate_copy2 = ctx->Nstate; + dpptrs_(&uplo, &Nstate_copy, &NmeasInChunk, + ctx->factorization_dense, + out, + &Nstate_copy2, &info, 1); + return info==0; +} + +// Computes pinv(J) for a subset of measurements: inv(JtJ) * +// Jt[imeasurement0..imeasurement0+N-1]. Returns false if something failed. +// ASSUMES THAT THE CHOLESKY FACTORIZATION HAS ALREADY BEEN COMPUTED. +// +// allocates memory, returns NULL on failure. ON SUCCESS, THE CALLER IS +// RESPONSIBLE FOR FREEING THE RETURNED MEMORY +// +// This function is experimental, and subject to change +static cholmod_dense* pseudoinverse_J_sparse(// inputs + const dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx, + int i_meas0, int NmeasInChunk, + + // Pre-allocated array for the + // right-hand-side. This will be used as a + // workspace. Create this like so: + // + // cholmod_allocate_dense( Nstate, + // NmeasInChunk, + // Nstate, + // CHOLMOD_REAL, + // &ctx->common ); + + cholmod_dense* Jt_chunk) +{ + // I'm solving JtJ x = b where J is sparse, b is sparse, but x ends up dense. + // cholmod doesn't have functions for this exact case. so I use the + // dense-sparse-dense function (cholmod_solve), and densify the input. Instead + // of sparse-sparse-sparse and the densifying the output (cholmod_spsolve). + // This feels like it'd be more efficient + + memset( Jt_chunk->x, 0, Jt_chunk->nrow*Jt_chunk->ncol*sizeof(double) ); + int Jt_chunk_ncol_backup = Jt_chunk->ncol; + for(int i_meas=0; i_meas= (int)ctx->Nmeasurements ) + { + // at the end, we could have one chunk with less that chunk_size + // columns + Jt_chunk->ncol = i_meas; + break; + } + + for(unsigned int i0=P(point->Jt, i_meas0+i_meas); i0Jt, i_meas0+i_meas+1); i0++) + { + int irow = I(point->Jt,i0); + double x = X(point->Jt,i0); + ((double*)Jt_chunk->x)[irow + i_meas*Jt_chunk->nrow] = x; + } + } + + // solve inv(JtJ)Jt[slice] + cholmod_dense* pinv = + cholmod_solve(CHOLMOD_A, + ctx->factorization, + Jt_chunk, + &ctx->common); + Jt_chunk->ncol = Jt_chunk_ncol_backup; + return pinv; +} + + +/* +The below is a bunch of code for outlier detection/rejection and for confidence +evaluation. IT'S ALL EXPERIMENTAL AND SUBJECT TO CHANGE + +What is an outlier? Suppose I just found an optimum. I define an outlier as an +observation that does two things to the problem if I remove that observation: + +1. The cost function would improve significantly. Things would clearly improve + because the cost function contribution of the removed point itself would get + removed, but ALSO because the parameters could fit the remaining data better + without that extra observation; the "outlier" wouldn't pull the solution away + from the "true" optimum. + +2. The confidence of the solution does not significantly decrease. One could + imagine a set of data that define the problem poorly, and produce a low cost + function value for some (overfit) set of parameters. And one can imagine an + extra point being added that defines the problem and increases the confidence + of the solution. This extra point would suppress the overfitting, so this + extra point would increase the cost function value. Condition 1 above would + make this extra point look like an outlier, and this condition is meant to + detect this case and to classify this point as NOT an outlier + +Let's say we just computed an optimum least-squares fit, and we try to determine +if some of the data points look like outliers. The least squares problem I just +solved has cost function + + E = norm2(x) + +where x is a length-N vector of measurements. We solved it by optimizing the +vector of parameters p. We're at an optimum, so dE/dp = 0 -> Jt x = 0 + +We define an outlier as a measurement that would greatly improve the cost +function E if this measurement was removed, and the problem was re-optimized. + +Let's say the problem is locally linear (J = dx/dp is constant), and let's say +re-optimizing moves the parameter by dp. The original cost function was + + E0 = norm2(x) + +If we move by dp, and take away the outlier's own cost, we get + + E1 = norm2(x + J dp) - norm2( x* + J* dp ) + +where x* and J* refer to the outlier measurements. Thus "Dima's self+others +factor" is norm2(x) - (norm2(x + J dp) - norm2( x* + J* dp )) + +We can choose to look at ONLY the effect on the other variables. That would +produce "Dima's others factor" = norm2(x)-norm2(x*) - (norm2(x + J dp) - +norm2(x* + J* dp )) + +This is very similar to Cook's D factor (which Dima apparently reinvented 40 +years after the fact!). This factor looks not at the difference of norm2(x), but +at norm2(difference) instead. So "Cook's self+others factor" is proportional to +norm2(x - (x+J dp)). This is the "normal" Cook's D factor. We can also compute +"Cook's others factor": norm2(x - vec(x*) - (x + J dp -(vec(x*) + full(J*) dp))) += norm2( - J dp + full(J*) dp) = norm2(J dp) + norm2(full(J*) dp) -2 dpt(Jt +full(J*) + full(J*)tJ)dp = norm2(J dp) + norm2(full(J*) dp) -2 dpt(Jt full(J*) + +full(J*)tJ)dp = norm2(J dp) + norm2(J* dp) - 2 norm2(J* dp) = norm2(J dp) - +norm2(J* dp) + +This is 4 flavors of a very similar computation. In summary (and ignoring scale +factors): + + Dima's self+others: -norm2(J dp) + 2 x*t J* dp + norm2(J* dp) + norm2(x*) + Dima's others : -norm2(J dp) + 2 x*t J* dp + norm2(J* dp) + Cook's self+others: norm2(J dp) + Cook's others : norm2(J dp) - norm2(J* dp) + +Let's compute these. dE1/dp = 0 at p+dp -> + + 0 = Jt x + JtJ dp - J*t x* - J*tJ* dp + = JtJ dp - J*t x* - J*tJ* dp + +-> dp = inv(JtJ - J*tJ*) J*t x* + +Woodbury identity: + + inv(JtJ - J*t J*) = + = inv(JtJ) - inv(JtJ) J*t inv(-I + J* inv(JtJ) J*t) J* inv(JtJ) + +Let + A = J* inv(JtJ) J*t + B = inv(A - I) + +So + AB = BA = I+B + +Thus + inv(JtJ - J*t J*) = + = inv(JtJ) - inv(JtJ) J*t B J* inv(JtJ) + +and + + dp = inv(JtJ - J*tJ*) J*t x* = + = inv(JtJ)J*t x* - inv(JtJ) J*t B J* inv(JtJ) J*t x* + = inv(JtJ)J*t x* - inv(JtJ) J*t B A x* + = inv(JtJ)J*t(I - B A) x* + = -inv(JtJ)J*t B x* + +Then + + norm2(J dp) = x*t ( B J* inv() Jt J inv() J*t B ) x* + = x*t ( B J* inv() J*t B ) x* + = x*t ( B A B ) x* + = x*t ( B + B*B ) x* + + 2 x*t J* dp = -2 x*t J* inv(JtJ)J*t B x* = + = -2 x*t A B x* = + = x*t (-2AB) x* = + = x*t (-2I - 2B) x* + + norm2(J* dp) = x*t ( B J* inv() J*tJ* inv() J*t B ) x* = + = x*t ( B A A B ) x* = + = x*t ( I + 2B + B*B ) x* + + norm2(x*) = x*t ( I ) x* + +There're two ways to compute the "Dima's self-only" factor. I can simply say +that the measurements' cost of norm2(x*) has been removed, so the factor is x*t +I x* or I can + +1. Remove measurements +2. Re-optimize +3. Look to see how the residual of x* changes + +This is different because I look at what happens to x*(p) when x* is no longer +in the optimized set. + +State moves by dp. x* moves by J* dp. I look at + + dE = norm2(x* + J* dp) - norm2(x*) + = 2 x*t J* dp + norm2(J* dp) = + = x*t (-2I - 2B + I + 2B + B*B ) x* = + = x*t (B*B - I) x* + +I expect that removing measurements from the optimization should make their +residuals worse. I.e. I expect dE > 0. Let's check. Is B*B - I positive +definite? Let's say that there's v,l such that + + (B*B-I)v = l v, norm2(v) = 1 + --> + BBv = (l+1)v + vBBv = l+1 + + Let u = Bv -> + norm2(u) = l+1 + + A = J* inv(JtJ) J*t + B = inv(A - I) -> + + v = (A-I)u + norm2(v) = 1 = norm2(Au) - 2ut A u + norm2(u) -> + -> l = 2ut A u - norm2(Au) + + Let w = J*t u + -> Au = J* inv(JtJ) w + -> ut A u = wt inv(JtJ) w -> + l = wt ( 2inv(JtJ) - inv(JtJ)J*tJ* inv(JtJ) ) w + + J*t J* = sum(outer(j*,j*)) = sum(outer(j,j)) - sum(outer(jnot*,jnot*)) = + = JtJ - Jnot*t Jnot* + + -> l = wt ( 2inv(JtJ) - inv(JtJ)JtJinv(JtJ) + inv(JtJ)Jnot*tJnot* inv(JtJ) ) w + = wt ( inv(JtJ) + inv(JtJ)Jnot*tJnot* inv(JtJ) ) w + + -> substitute -> l = wt ( C + CDC ) w + + where C >= 0 and D >= 0 + + wt C wt >= 0 + + wt CDC wt = (Ctw)t D Ctw >= 0 -> l >= 0 + +So B*B-I >= 0 and dE >= 0: removing a point will never make its own fit better + +- if dE >> 0: the other data does NOT support this measurement being correct +- if dE ~~ 0: the other data supports this measurement being correct + + +Putting all this together I get the expressions for the factors above: + + Dima's self+others: x*t (-B ) x* + Dima's others : x*t (-B - I ) x* + Dima's self (simple): x*t ( I ) x* + Dima's self (interesting): x*t (B*B - I ) x* + Cook's self+others: x*t ( B + B*B) x* + Cook's others : x*t (-B - I ) x* + + + +One can also do a similar analysis to gauge our confidence in a solution. We can +do this by + +1. Solving the optimization problem + +2. Querying the solution in a way we care about to produce a new feature + group of measurements x = f - ref. We can compute J = dx/dp = df/dp. And + we presumably know something about ref: like its probability + distribution for instance. Example: we just calibrated a camera; we want + to know how confident we are about a projection in a particular spot on + the imager. I compute a projection in that spot: q = project(v). If + added to the optimization I'd get x = q - ref where 'ref' would be the + observed pixel coordinate. + +3. If this new feature was added to the optimization, I can compute its + outlierness factor in the same way as before. If we are confident in the + solution in a particular spot, then we have consensus, and it wouldn't take + much for these queries to look like outliers: the expected value of the + outlierness would be high. Conversely, if we aren't well-defined then a wider + set of points would fit the solution, and things wouldn't look very outliery + +Very similar analysis to the above: + +Let p,x,J represent the solution. The new feature we're adding is x* with +jacobian J*. The solution would move by dp to get to the new optimum. + +Original solution is an optimum: Jt x = 0 + +If we add x* and move by dp, we get + + E1 = norm2(x + J dp) + norm2( x* + J* dp ) + +Thus "Dima's self+others factor" is (norm2(x + J dp) + norm2( x* + J* dp )) - +norm2(x) = norm2(Jdp) + norm2( x* + J* dp ) + +We can choose to look at ONLY the effect on the other variables. That would +produce "Dima's others factor" norm2(x + J dp) - norm2(x) = norm2(Jdp) + +This is very similar to Cook's D factor (which Dima apparently reinvented 40 +years after the fact!). This factor looks not at the difference of norm2(x), but +at norm2(difference) instead. So "Cook's others factor" is proportional to +norm2(x - (x+J dp)). This is the "normal" Cook's D factor. We can also compute +"Cook's self+others factor": norm2(concat(x + J dp, x* + J* dp) - concat(x,x*)) += norm2(x + J dp-x) + norm2(x* + J* dp - x*) = norm2(J dp) + norm2(J* dp) + +This is 4 flavors of a very similar computation. In summary (and ignoring scale +factors): + + Dima's self+others: norm2(J dp) + 2 x*t J* dp + norm2(J* dp) + norm2(x*) + Dima's others : norm2(J dp) + Cook's self+others: norm2(J dp) + norm2(J* dp) + Cook's others : norm2(J dp) + +The problem including the new point is also at an optimum: + + E1 = norm2(x + J dp) + norm2( x* + J* dp ) + dE1/dp = 0 -> 0 = Jt x + JtJ dp + J*t x* + J*tJ*dp = + = JtJ dp + J*t x* + J*tJ*dp +-> dp = -inv(JtJ + J*tJ*) J*t x* + +Woodbury identity: + + -inv(JtJ + J*t J*) = + = -inv(JtJ) + inv(JtJ) J*t inv(I + J* inv(JtJ) J*t) J* inv(JtJ) + +Let + A = J* inv(JtJ) J*t (same as before) + B = inv(A + I) (NOT the same as before) + +So + AB = BA = I-B + +Thus + -inv(JtJ + J*t J*) = + = -inv(JtJ) + inv(JtJ) J*t B J* inv(JtJ) + +and + + dp = -inv(JtJ + J*tJ*) J*t x* = + = -inv(JtJ)J*t x* + inv(JtJ) J*t B J* inv(JtJ) J*t x* = + = -inv(JtJ)J*t x* + inv(JtJ) J*t B A x* + = -inv(JtJ)J*t(I - B A) x* + = -inv(JtJ)J*t B x* (same as before, but with a different B!) + +Then + + norm2(J dp) = x*t ( B J* inv() Jt J inv() J*t B ) x* + = x*t ( B J* inv() J*t B ) x* + = x*t ( B A B ) x* + = x*t ( B - B*B ) x* + + 2 x*t J* dp = -2 x*t J* inv(JtJ)J*t B x* = + = -2 x*t A B x* = + = x*t (-2AB) x* = + = x*t (-2I + 2B) x* + + norm2(J* dp) = x*t ( B J* inv() J*tJ* inv() J*t B ) x* = + = x*t ( B A A B ) x* = + = x*t ( I - 2B + B*B ) x* + + norm2(x*) = x*t ( I ) x* + +How do I compute "Dima's self" factor? The "simple" flavor from above looks at +the new measurement only: norm2(x*). The "interesting" flavor, look at what +happens to the measurements' error when they're added to the optimization set. +State moves by dp. x* moves by J* dp. I look at + + dE = norm2(x* + J*dp) - norm2(x*) = + 2 x*t J* dp + norm2(J* dp) = + x*t (-2I + 2B + I - 2B + B*B) x* = + x*t (B*B - I) x* + +I expect that adding a point to the optimization would make it fit better: dE < +0. Let's check. Let's say that there's v,l such that + + (B*B-I)v = l v, norm2(v) = 1 + --> + BBv = (l+1)v + vBBv = l+1 + + Let u = Bv -> + norm2(u) = l+1 + + A = J* inv(JtJ) J*t + B = inv(A + I) -> + + v = (A+I)u + norm2(v) = 1 = norm2(Au) + 2ut A u + norm2(u) -> + -> l = -2ut A u - norm2(Au) + + Let w = J*t u + -> Au = J* inv(JtJ) w + -> ut A u = wt inv(JtJ) w -> + l = -2 wt inv(JtJ) w - norm2(Au) + + Since inv(JtJ) > 0 -> l < 0. As expected + +So B*B-I is negative definite: adding measurements to the optimization set makes +them fit better + +Putting all this together I get the expressions for the factors above: + + Dima's self+others: x*t (B ) x* + Dima's others : x*t (B - B*B) x* + Dima's self (simple): x*t (I ) x* + Dima's self (interesting): x*t (B*B - I) x* + Cook's self+others: x*t (I - B ) x* + Cook's others : x*t (B - B*B) x* + +These expressions are all tested and verified in +mrcal/analyses/outlierness-test.py + + +There're several slightly-different definitions of Cook's D and of a +rule-of-thumb threshold floating around on the internet. Wikipedia says: + + D = norm2(x_io - x_i)^2 / (Nstate * norm2(x_io)/(Nmeasurements - Nstate)) + D_threshold = 1 + +An article https://www.nature.com/articles/nmeth.3812 says + + D = norm2(x_io - x_i)^2 / ((Nstate+1) * norm2(x_io)/(Nmeasurements - Nstate -1)) + D_threshold = 4/Nmeasurements + +I haven't tracked down the reference for this second definition, but it looks +more reasonable, and I use it here. + +Here I use the second definition. That definition expands to + + k = xo^2 / ((Nstate+1) * norm2(x)/(Nmeasurements - Nstate -1)) + B = 1.0/(jt inv(JtJ) j - 1) + f = k * (B + B*B) + +I report normalized outlierness factors so that the threshold is 1. Thus I use + + k = Nmeasurements / (4*((Nstate+1) * norm2(x)/(Nmeasurements - Nstate - 1))) + +*/ + + +static void accum_outlierness_factor(// output + double* factor, + + // inputs + const double* x, + + // A is symmetric. I store the upper triangle row-first + const double* A, + + // if outliers are grouped into features, the + // feature size is set here + int featureSize, + double k) +{ + // This implements Dima's self+outliers factor. + // + // from the derivation in a big comment above I have + // + // f = k (x*t (-B) x* ) + // + // where B = inv(A - I) and + // A = J* inv(JtJ) J*t + + // I only implemented featureSize == 1 and 2 so far. + if(featureSize <= 1) + { + featureSize = 1; + + double denom = 1.0 - *A; + if( fabs(denom) < 1e-8 ) + { + *factor = DBL_MAX; // definitely an outlier + return; + } + else + *factor = x[0]*x[0] / denom; + } + else if(featureSize == 2) + { + double det = (1.0-A[0])*(1.0-A[2]) - A[1]*A[1]; + if( fabs(det) < 1e-8 ) + { + *factor = DBL_MAX; // definitely an outlier + return; + } + else + { + double B00_det = A[2] - 1.0; + double B11_det = A[0] - 1.0; + double B01_det = -A[1]; + + // inner(x, Bx) + double xBx = + (x[0]*x[0]*B00_det + + x[0]*x[1]*B01_det * 2.0 + + x[1]*x[1]*B11_det) / det; + + // norm2(Bx) + double v1 = x[0]*B00_det + x[1]*B01_det; + double v2 = x[0]*B01_det + x[1]*B11_det; + double xBBx = (v1*v1 + v2*v2) / (det*det); + + // // mine self+others + // *factor = -xBx; + + // // mine others / cook others + // *factor = -(x[0]*x[0] + x[1]*x[1]) - xBx; + + // cook's self+others + *factor = xBx + xBBx; + + } + } + else + { + SAY("featureSize > 2 not implemented yet. Got featureSize=%d", featureSize); + ASSERT(0); + } + + +// #warning This is a hack. The threshold should be 1.0, and the scaling should make sure that is the case. I am leaving it for now + k /= 8.; + + + *factor *= k; +} + +static void getOutliernessScale(// out + double* scale, // if *scale > 0, I just keep what I have + + // in + int Nmeasurements, int Nstate, + int NoutlierFeatures, int featureSize, + double norm2_x) +{ + // if *scale > 0, I just keep what I have + if(*scale > 0.0) + return; + + int Nmeasurements_outliers = NoutlierFeatures*featureSize; + int Nmeasurements_nonoutliers = Nmeasurements - Nmeasurements_outliers; + + *scale = + (double)(Nmeasurements_nonoutliers) / + (4.*((double)(Nstate+1) * norm2_x/(double)(Nmeasurements_nonoutliers - Nstate - 1))); +} + +static bool getOutliernessFactors_dense( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, + // the feature size is set here + int featureSize, + int Nfeatures, + int NoutlierFeatures, + const dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ) +{ + // cholmod_spsolve() and cholmod_solve()) work in chunks of 4, so I do this in + // chunks of 4 too. I pass it rows of J, 4 at a time. Note that if I have + // measurement features, I don't want these to cross chunk boundaries, so I set + // up chunk_size=lcm(N,4) + int chunk_size = 4; + if(featureSize <= 1) + featureSize = 1; + if(featureSize > 1) + { + // haven't implemented anything else yet. Don't have lcm() readily available + ASSERT(featureSize == 2); + // chunk_size = lcm(chunk_size,featureSize) + } + + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + bool result = false; + + double* invJtJ_Jt = static_cast(malloc(Nstate*chunk_size*sizeof(double))); + if(invJtJ_Jt == NULL) + { + SAY("Couldn't allocate invJtJ_Jt!"); + free(invJtJ_Jt); + return result; + } + + getOutliernessScale(scale, + Nmeasurements, Nstate, NoutlierFeatures, featureSize, point->norm2_x); + + int i_measurement_valid_chunk_start = -1; + int i_measurement_valid_chunk_last = -1; + int i_measurement = 0; + for( int i_feature=0; i_feature i_measurement_valid_chunk_last ) + { + bool pinvresult = pseudoinverse_J_dense(invJtJ_Jt, point, ctx, + i_measurement, chunk_size); + if(!pinvresult) + { + SAY("Couldn't compute pinv!"); + free(invJtJ_Jt); + return result; + } + i_measurement_valid_chunk_start = i_measurement; + i_measurement_valid_chunk_last = i_measurement+chunk_size-1; + } + + // from the derivation in a big comment in above I have + // + // f = scale (xot (...) xo ) + // + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle + std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; iJ_dense[Nstate* i_measurement+j + k]; + } + accum_outlierness_factor(&factors[i_feature], + &point->x[i_measurement], + A.data(), featureSize, *scale); + } + + result = true; + free(invJtJ_Jt); + return result; +} + + +static bool getOutliernessFactors_sparse( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, + // the feature size is set here + int featureSize, + int Nfeatures, + int NoutlierFeatures, + const dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ) +{ + // cholmod_spsolve() and cholmod_solve()) work in chunks of 4, so I do this in + // chunks of 4 too. I pass it rows of J, 4 at a time. Note that if I have + // measurement features, I don't want these to cross chunk boundaries, so I set + // up chunk_size=lcm(N,4) + int chunk_size = 4; + if(featureSize <= 1) + featureSize = 1; + if(featureSize > 1) + { + // haven't implemented anything else yet. Don't have lcm() readily available + ASSERT(featureSize == 2); + // chunk_size = lcm(chunk_size,featureSize) + } + + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + bool result = false; + + cholmod_dense* invJtJ_Jt = NULL; + cholmod_dense* Jt_chunk = + cholmod_allocate_dense( Nstate, + chunk_size, + Nstate, + CHOLMOD_REAL, + &ctx->common ); + if(!Jt_chunk) + { + SAY("Couldn't allocate Jt_chunk!"); + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; + } + + getOutliernessScale(scale, + Nmeasurements, Nstate, NoutlierFeatures, featureSize, point->norm2_x); + + int i_measurement_valid_chunk_start = -1; + int i_measurement_valid_chunk_last = -1; + int i_measurement = 0; + for( int i_feature=0; i_feature i_measurement_valid_chunk_last ) + { + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + invJtJ_Jt = pseudoinverse_J_sparse(point, ctx, + i_measurement, chunk_size, + Jt_chunk); + if(invJtJ_Jt == NULL) + { + SAY("Couldn't compute pinv!"); + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; + } + + i_measurement_valid_chunk_start = i_measurement; + i_measurement_valid_chunk_last = i_measurement+chunk_size-1; + } + + // from the derivation in a big comment in above I have + // + // f = scale (xot (...) xo ) + // + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle + std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; iJt, i_measurement+j); + l < P(point->Jt, i_measurement+j+1); + l++) + { + int k = I(point->Jt, l); + A[iA] += + ((double*)invJtJ_Jt->x)[Nstate*(i_measurement+i-i_measurement_valid_chunk_start) + k] * + X(point->Jt, l); + } + } + accum_outlierness_factor(&factors[i_feature], + &point->x[i_measurement], + A.data(), featureSize, *scale); + } + + result = true; + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; +} + +bool dogleg_getOutliernessFactors( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, the + // feature size is set here + int featureSize, + int Nfeatures, + int NoutlierFeatures, // how many outliers we already have + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ) +{ + if(featureSize <= 1) + featureSize = 1; + + dogleg_computeJtJfactorization( point, ctx ); + bool result; + if(ctx->is_sparse) + result = getOutliernessFactors_sparse(factors, scale, featureSize, Nfeatures, NoutlierFeatures, point, ctx); + else + result = getOutliernessFactors_dense(factors, scale, featureSize, Nfeatures, NoutlierFeatures, point, ctx); + +#if 0 + if( result ) + { + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + + + + static FILE* fp = NULL; + if(fp == NULL) + fp = fopen("/tmp/check-outlierness.py", "w"); + static int count = -1; + count++; + if(count > 5) + goto done; + + fprintf(fp, "# WARNING: all J here are unscaled with SCALE_....\n"); + + if(count == 0) + { + fprintf(fp, + "import numpy as np\n" + "import numpysane as nps\n" + "np.set_printoptions(linewidth=100000)\n" + "\n"); + } + + fprintf(fp, "x%d = np.array((", count); + for(int j=0;jx[j]); + fprintf(fp,"))\n"); + + if( ctx->is_sparse ) + { + fprintf(fp, "J%d = np.zeros((%d,%d))\n", count, Nmeasurements, Nstate); + for(int imeas=0;imeasJt, imeas); + j < (int)P(point->Jt, imeas+1); + j++) + { + int irow = I(point->Jt, j); + fprintf(fp, "J%d[%d,%d] = %.20g\n", count, + imeas, irow, X(point->Jt, j)); + } + } + } + else + { + fprintf(fp, "J%d = np.array((\n", count); + for(int j=0;jJ_dense[j*Nstate+i]); + fprintf(fp, "),\n"); + } + fprintf(fp,"))\n"); + } + + fprintf(fp, "Nmeasurements = %d\n", Nmeasurements); + fprintf(fp, "Nstate = %d\n", Nstate); + fprintf(fp, "Nfeatures = %d\n", Nfeatures); + fprintf(fp, "featureSize = %d\n", featureSize); + fprintf(fp, "NoutlierFeatures = %d\n", NoutlierFeatures); + + fprintf(fp, "scale_got = %.20f\n", *scale); + + fprintf(fp, "factors_got = np.array(("); + for(int j=0;j + B = inv(I + A) -> + tr(B) = (2+a00+a11) / ((1+a00)*(1+a11) - a01^2) + */ + + + // This is Jt because cholmod thinks in terms of col-first instead of + // row-first + std::vector Jt_p(featureSize+1); + std::vector Jt_i(NstateActive*featureSize); + for(int i=0; i<=featureSize; i++) + { + Jt_p[i] = i*NstateActive; + if(i==featureSize) break; + for(int j=0; j(ctx->Nstate), + .ncol = static_cast(featureSize), + .nzmax = static_cast(NstateActive*featureSize), + .p = Jt_p.data(), + .i = Jt_i.data(), + .x = const_cast(JqueryFeature), + .stype = 0, // NOT symmetric + .itype = CHOLMOD_INT, + .xtype = CHOLMOD_REAL, + .dtype = CHOLMOD_DOUBLE, + .sorted = 1, + .packed = 1}; + + // Really shouldn't need to do this every time. In fact I probably don't need + // to do it at all, since this will have been done by the solver during the + // last step + dogleg_computeJtJfactorization( point, ctx ); + cholmod_sparse* invJtJ_Jp = + cholmod_spsolve(CHOLMOD_A, + ctx->factorization, + &Jt_query_sparse, + &ctx->common); + + // Now I need trace(matmult(Jquery, invJtJ_Jp)) + + // haven't implemented anything else yet + ASSERT(featureSize == 2); + double A[4] = {}; // gah. only elements 0,1,3 will be stored. + + for(int i=0; i= istateActive) + { + if(row >= istateActive+NstateActive) + break; + + int ic0 = i*featureSize; + for(int k=i; kcommon); + + double invB00 = A[0]+1.0; + double invB01 = A[1]; + double invB11 = A[3]+1.0; + + double det_invB_recip = 1.0/(invB00*invB11 - invB01*invB01); + double B00 = invB11 * det_invB_recip; + double B11 = invB00 * det_invB_recip; + + + double B01 = -invB01 * det_invB_recip; + double traceB = B00 + B11; + + double traceBB = B00*B00 + 2.0*B01*B01 + B11*B11; + + +#if 0 + static int count = -1; + count++; + if(count <= 5) + { + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + + + + static FILE* fp = NULL; + if(fp == NULL) + fp = fopen("/tmp/check-query-outlierness.py", "w"); + + fprintf(fp, "# WARNING: all J here are unscaled with SCALE_....\n"); + + if(count == 0) + { + fprintf(fp, + "import numpy as np\n" + "import numpysane as nps\n" + "np.set_printoptions(linewidth=100000)\n" + "\n"); + } + + fprintf(fp, "x%d = np.array((", count); + for(int j=0;jx[j]); + fprintf(fp,"))\n"); + + if( ctx->is_sparse ) + { + fprintf(fp, "J%d = np.zeros((%d,%d))\n", count, Nmeasurements, Nstate); + for(int imeas=0;imeasJt, imeas); + j < (int)P(point->Jt, imeas+1); + j++) + { + int irow = I(point->Jt, j); + fprintf(fp, "J%d[%d,%d] = %g\n", count, + imeas, irow, X(point->Jt, j)); + } + } + } + else + { + fprintf(fp, "J%d = np.array((\n", count); + for(int j=0;jJ_dense[j*Nstate+i]); + fprintf(fp, "),\n"); + } + fprintf(fp,"))\n"); + } + + fprintf(fp, "Nmeasurements = %d\n", Nmeasurements); + fprintf(fp, "Nstate = %d\n", Nstate); + fprintf(fp, "featureSize = %d\n", featureSize); + fprintf(fp, "traceB_got = %.20g\n", traceB); + + fprintf(fp, "Jq = np.zeros((featureSize, Nstate), dtype=float)\n"); + for(int i=0; iNmeasurements; + int Nstate = ctx->Nstate; + + double scale = -1.0; + getOutliernessScale(&scale, + Nmeasurements, Nstate, NoutlierFeatures, featureSize, point->norm2_x); + + // // Dima's self+others + // return scale * traceB; + + // Cook's self+others + return scale * (2.0 - traceB); + + // // Dima's others/Cook's others + // // This one is non-monotonic in outlierness-test + // return scale * (traceB - traceBB); + +} + + +#define OUTLIER_CONFIDENCE_DROP_THRESHOLD 0.05 +bool dogleg_markOutliers(// output, input + struct dogleg_outliers_t* markedOutliers, + double* scale, // if <0 then I recompute + + // output, input + int* Noutliers, + + // input + double (getConfidence)(int i_feature_exclude), + + // if outliers are grouped into features, the feature + // size is set here + int featureSize, + int Nfeatures, + + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx) +{ + if(featureSize <= 1) + featureSize = 1; + + bool markedAny = false; + + double* factors = static_cast(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); + free(factors); + return markedAny; + } + + if(!dogleg_getOutliernessFactors(factors, scale, + featureSize, Nfeatures, + *Noutliers, + point, ctx)) + { + free(factors); + return markedAny; + } + + // I have my list of POTENTIAL outliers (any that have factor > 1.0). I + // check to see how much confidence I would lose if I were to throw out any + // of these measurements, and accept the outlier ONLY if the confidence loss + // is acceptable + double confidence0 = getConfidence(-1); + if( confidence0 < 0.0 ) + { + free(factors); + return markedAny; + } + + SAY_IF_VERBOSE("Initial confidence: %g", confidence0); + + *Noutliers = 0; + for(int i=0; i(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); + free(factors); + } + + dogleg_getOutliernessFactors(factors, scale, + featureSize, Nfeatures, Noutliers, point, ctx); + + SAY("## Outlier statistics"); + SAY("# i_feature outlier_factor confidence_drop_relative_if_removed"); + + double confidence_full = getConfidence(-1); + + for(int i=0; i vout +static inline void add_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i vout +static inline void sub_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i vout +static inline void add_vec_vout_scaled(int n, const double* restrict a, const double* restrict b, double* restrict vout, double scale) +{ + for(int i=0; i vout +static inline void sub_vec_vout_scaled(int n, const double* restrict a, const double* restrict b, double* restrict vout, double scale) +{ + for(int i=0; i a +static inline void add_vec(int n, double* restrict a, const double* restrict b) +{ + for(int i=0; i a +static inline void sub_vec(int n, double* restrict a, const double* restrict b) +{ + for(int i=0; i a +static inline void add_vec_scaled(int n, double* restrict a, const double* restrict b, double scale) +{ + for(int i=0; i a +static inline void sub_vec_scaled(int n, double* restrict a, const double* restrict b, double scale) +{ + for(int i=0; i + vaccum +static inline void add_vec_vaccum(int n, const double* restrict a, const double* restrict b, double* restrict vaccum) +{ + for(int i=0; i + vaccum +static inline void sub_vec_vaccum(int n, const double* restrict a, const double* restrict b, double* restrict vaccum) +{ + for(int i=0; i +#include +#include "strides.h" + +template struct vec_withgrad_t; + + +template +struct val_withgrad_t +{ + double x; + double j[NGRAD == 0 ? 1 : NGRAD]; + + + val_withgrad_t(double _x = 0.0) : x(_x) + { + for(int i=0; i operator+( const val_withgrad_t& b ) const + { + val_withgrad_t y = *this; + y.x += b.x; + for(int i=0; i operator+( double b ) const + { + val_withgrad_t y = *this; + y.x += b; + return y; + } + + void operator+=( const val_withgrad_t& b ) + { + *this = (*this) + b; + } + + val_withgrad_t operator-( const val_withgrad_t& b ) const + { + val_withgrad_t y = *this; + y.x -= b.x; + for(int i=0; i operator-( double b ) const + { + val_withgrad_t y = *this; + y.x -= b; + return y; + } + + void operator-=( const val_withgrad_t& b ) + { + *this = (*this) - b; + } + + val_withgrad_t operator-() const + { + return (*this) * (-1); + } + + val_withgrad_t operator*( const val_withgrad_t& b ) const + { + val_withgrad_t y; + y.x = x * b.x; + for(int i=0; i operator*( double b ) const + { + val_withgrad_t y; + y.x = x * b; + for(int i=0; i& b ) + { + *this = (*this) * b; + } + + void operator*=( const double b ) + { + *this = (*this) * b; + } + + val_withgrad_t operator/( const val_withgrad_t& b ) const + { + val_withgrad_t y; + y.x = x / b.x; + for(int i=0; i operator/( double b ) const + { + return (*this) * (1./b); + } + + void operator/=( const val_withgrad_t& b ) + { + *this = (*this) / b; + } + + void operator/=( const double b ) + { + *this = (*this) / b; + } + + val_withgrad_t sqrt(void) const + { + val_withgrad_t y; + y.x = ::sqrt(x); + for(int i=0; i square(void) const + { + val_withgrad_t s; + s.x = x*x; + for(int i=0; i sin(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + val_withgrad_t y; + y.x = s; + for(int i=0; i cos(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + val_withgrad_t y; + y.x = c; + for(int i=0; i sincos(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + vec_withgrad_t sc; + sc.v[0].x = s; + sc.v[1].x = c; + for(int i=0; i tan(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + val_withgrad_t y; + y.x = s/c; + for(int i=0; i atan2(val_withgrad_t& x) const + { + val_withgrad_t th; + const val_withgrad_t& y = *this; + + th.x = ::atan2(y.x, x.x); + // dth/dv = d/dv atan2(y,x) + // = d/dv atan(y/x) + // = 1 / (1 + y^2/x^2) d/dv (y/x) + // = x^2 / (x^2 + y^2) / x^2 * (dy/dv x - y dx/dv) + // = 1 / (x^2 + y^2) * (dy/dv x - y dx/dv) + double norm2 = y.x*y.x + x.x*x.x; + for(int i=0; i asin(void) const + { + val_withgrad_t th; + th.x = ::asin(x); + double dasin_dx = 1. / ::sqrt( 1. - x*x ); + for(int i=0; i acos(void) const + { + val_withgrad_t th; + th.x = ::acos(x); + double dacos_dx = -1. / ::sqrt( 1. - x*x ); + for(int i=0; i sinx_over_x(// To avoid recomputing it + const val_withgrad_t& sinx) const + { + // For small x I need special-case logic. In the limit as x->0 I have + // sin(x)/x -> 1. But I'm propagating gradients, so I need to capture + // that. I have + // + // d(sin(x)/x)/dx = + // (x cos(x) - sin(x))/x^2 + // + // As x -> 0 this is + // + // (cos(x) - x sin(x) - cos(x)) / (2x) = + // (- x sin(x)) / (2x) = + // -sin(x) / 2 = + // 0 + // + // So for small x the gradient is 0 + if(fabs(x) < 1e-5) + return val_withgrad_t(1.0); + + return sinx / (*this); + } +}; + + +template +struct vec_withgrad_t +{ + val_withgrad_t v[NVEC == 0 ? 1 : NVEC]; + + vec_withgrad_t() {} + + + void init_vars(const double* x_in, int ivar0, int Nvars, int i_gradvec0 = -1, + int stride = sizeof(double)) + { + // Initializes vector entries ivar0..ivar0+Nvars-1 inclusive using the + // data in x_in[]. x_in[0] corresponds to vector entry ivar0. If + // i_gradvec0 >= 0 then vector ivar0 corresponds to gradient index + // i_gradvec0, with all subsequent entries being filled-in + // consecutively. It's very possible that NGRAD > Nvars. Initially the + // subset of the gradient array corresponding to variables + // i_gradvec0..i_gradvec0+Nvars-1 is an identity, with the rest being 0 + memset((char*)&v[ivar0], 0, Nvars*sizeof(v[0])); + for(int i=ivar0; i= 0) + v[i].j[i_gradvec0+i-ivar0] = 1.0; + } + } + + + vec_withgrad_t(const double* x_in, int i_gradvec0 = -1, + int stride = sizeof(double)) + { + init_vars(x_in, 0, NVEC, i_gradvec0, stride); + } + + + val_withgrad_t& operator[](int i) + { + return v[i]; + } + + + const val_withgrad_t& operator[](int i) const + { + return v[i]; + } + + + void operator+=( const vec_withgrad_t& x ) + { + (*this) = (*this) + x; + } + + vec_withgrad_t operator+( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) + x; + } + + vec_withgrad_t operator+( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator+( double x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) - x; + } + + vec_withgrad_t operator-( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) - x; + } + + vec_withgrad_t operator-( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator-( double x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) * x; + } + + vec_withgrad_t operator*( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) * x; + } + + vec_withgrad_t operator*( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator*( double x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) / x; + } + + vec_withgrad_t operator/( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) / x; + } + + vec_withgrad_t operator/( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator/( double x ) const + { + vec_withgrad_t p; + for(int i=0; i dot( const vec_withgrad_t& x) const + { + val_withgrad_t d; // initializes to 0 + for(int i=0; i e = x.v[i]*v[i]; + d += e; + } + return d; + } + + + vec_withgrad_t cross( const vec_withgrad_t& x) const + { + vec_withgrad_t c; + c[0] = v[1]*x.v[2] - v[2]*x.v[1]; + c[1] = v[2]*x.v[0] - v[0]*x.v[2]; + c[2] = v[0]*x.v[1] - v[1]*x.v[0]; + return c; + } + + + val_withgrad_t norm2(void) const + { + return dot(*this); + } + + + val_withgrad_t mag(void) const + { + val_withgrad_t l2 = norm2(); + return l2.sqrt(); + } + + + void extract_value(double* out, + int stride = sizeof(double), + int ivar0 = 0, int Nvars = NVEC) const + { + for(int i=ivar0; i +static +vec_withgrad_t +cross( const vec_withgrad_t& a, + const vec_withgrad_t& b ) +{ + vec_withgrad_t c; + c.v[0] = a.v[1]*b.v[2] - a.v[2]*b.v[1]; + c.v[1] = a.v[2]*b.v[0] - a.v[0]*b.v[2]; + c.v[2] = a.v[0]*b.v[1] - a.v[1]*b.v[0]; + return c; +} + +template +static +val_withgrad_t +cross_norm2( const vec_withgrad_t& a, + const vec_withgrad_t& b ) +{ + vec_withgrad_t c = cross(a,b); + return c.norm2(); +} + +template +static +val_withgrad_t +cross_mag( const vec_withgrad_t& a, + const vec_withgrad_t& b ) +{ + vec_withgrad_t c = cross(a,b); + return c.mag(); +} diff --git a/wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h b/wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h new file mode 100644 index 0000000000..13bd2e919f --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h @@ -0,0 +1,130 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +// A 2D point or vector +// +// The individual elements can be accessed via .x and .y OR the vector can be +// accessed as an .xy[] array: +// +// mrcal_point2_t p = f(); +// +// Now p.x and p.xy[0] refer to the same value. +typedef union +{ + struct + { + double x,y; + }; + + double xy[2]; +} mrcal_point2_t; + +// A 3D point or vector +// +// The individual elements can be accessed via .x and .y and .z OR the vector +// can be accessed as an .xyz[] array: +// +// mrcal_point3_t p = f(); +// +// Now p.x and p.xy[0] refer to the same value. +typedef union +{ + struct + { + double x,y,z; + }; + double xyz[3]; +} mrcal_point3_t; + +// Unconstrained 6DOF pose containing a Rodrigues rotation and a translation +typedef struct +{ + mrcal_point3_t r,t; +} mrcal_pose_t; + + +//////////// Easy convenience stuff +////// point2 + +static double mrcal_point2_inner(const mrcal_point2_t a, const mrcal_point2_t b) +{ + return + a.x*b.x + + a.y*b.y; +} + +static double mrcal_point2_norm2(const mrcal_point2_t a) +{ + return mrcal_point2_inner(a,a); +} +#define mrcal_point2_mag(a) sqrt(norm2(a)) // macro to not require #include + + +static mrcal_point2_t mrcal_point2_add(const mrcal_point2_t a, const mrcal_point2_t b) +{ + return (mrcal_point2_t){ .x = a.x + b.x, + .y = a.y + b.y }; +} + +static mrcal_point2_t mrcal_point2_sub(const mrcal_point2_t a, const mrcal_point2_t b) +{ + return (mrcal_point2_t){ .x = a.x - b.x, + .y = a.y - b.y }; +} + +static mrcal_point2_t mrcal_point2_scale(const mrcal_point2_t a, const double s) +{ + return (mrcal_point2_t){ .x = a.x * s, + .y = a.y * s }; +} +////// point3 + +static double mrcal_point3_inner(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return + a.x*b.x + + a.y*b.y + + a.z*b.z; +} + +static double mrcal_point3_norm2(const mrcal_point3_t a) +{ + return mrcal_point3_inner(a,a); +} +#define mrcal_point3_mag(a) sqrt(mrcal_point3_norm2(a)) // macro to not require #include + + +static mrcal_point3_t mrcal_point3_add(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return (mrcal_point3_t){ .x = a.x + b.x, + .y = a.y + b.y, + .z = a.z + b.z }; +} + +static mrcal_point3_t mrcal_point3_sub(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return (mrcal_point3_t){ .x = a.x - b.x, + .y = a.y - b.y, + .z = a.z - b.z }; +} + +static mrcal_point3_t mrcal_point3_scale(const mrcal_point3_t a, const double s) +{ + return (mrcal_point3_t){ .x = a.x * s, + .y = a.y * s, + .z = a.z * s }; +} + +static mrcal_point3_t mrcal_point3_cross(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return (mrcal_point3_t){ .x = a.y*b.z - a.z*b.y, + .y = a.z*b.x - a.x*b.z, + .z = a.x*b.y - a.y*b.x }; +} diff --git a/wpical/src/main/native/thirdparty/mrcal/include/cahvore.h b/wpical/src/main/native/thirdparty/mrcal/include/cahvore.h new file mode 100644 index 0000000000..4b6d3ddadd --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/cahvore.h @@ -0,0 +1,23 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +#include +#include "basic-geometry.h" + +bool project_cahvore_internals( // outputs + mrcal_point3_t* __restrict pdistorted, + double* __restrict dpdistorted_dintrinsics_nocore, + double* __restrict dpdistorted_dp, + + // inputs + const mrcal_point3_t* __restrict p, + const double* __restrict intrinsics_nocore, + const double cahvore_linearity); + diff --git a/wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h b/wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h new file mode 100644 index 0000000000..592e30694f --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h @@ -0,0 +1,1988 @@ +#pragma once + +// Extra functions I'm using in mrcal. I'm going to replace this whole library +// eventually, to make things nicer. These new functions will be a part of the +// replacement, and I'm not going to be thorough and I'm not going to add tests +// until I do that. + +#define restrict __restrict + +// Upper triangle is stored, in the usual row-major order. + +static +int index_sym33(int i, int j) +{ + // In the top-right triangle I have i<=j, and + // index = N*i+j - sum(i, i=0..i) + // = N*i+j - (i+1)*i/2 + // = (N*2 - i - 1)*i/2 + j + const int N=3; + if(i<=j) return (N*2-i-1)*i/2 + j; + else return (N*2-j-1)*j/2 + i; +} + +static +int index_sym33_assume_upper(int i, int j) +{ + const int N=3; + return (N*2-i-1)*i/2 + j; +} + +// Upper triangle is stored, in the usual row-major order. + +static +int index_sym66(int i, int j) +{ + // In the top-right triangle I have i<=j, and + // index = N*i+j - sum(i, i=0..i) + // = N*i+j - (i+1)*i/2 + // = (N*2 - i - 1)*i/2 + j + const int N=6; + if(i<=j) return (N*2-i-1)*i/2 + j; + else return (N*2-j-1)*j/2 + i; +} + +static +int index_sym66_assume_upper(int i, int j) +{ + const int N=6; + return (N*2-i-1)*i/2 + j; +} + +static +void mul_gen33_gen33insym66(// output + double* restrict P, int P_strideelems0, int P_strideelems1, + // input + const double* A, int A_strideelems0, int A_strideelems1, + const double* Bsym66, int B_i0, int B_j0, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + { + P[iout*P_strideelems0 + jout*P_strideelems1] = 0; + for(int k=0; k<3; k++) + { + P[iout*P_strideelems0 + jout*P_strideelems1] += + A[iout*A_strideelems0 + k*A_strideelems1] * + Bsym66[index_sym66(k+B_i0, jout+B_j0)]; + } + P[iout*P_strideelems0 + jout*P_strideelems1] *= scale; + } +} +// Assumes the output is symmetric, and only computes the upper triangle + +static +void mul_gen33_gen33_into33insym66_accum(// output + double* restrict Psym66, int P_i0, int P_j0, + // input + const double* A, int A_strideelems0, int A_strideelems1, + const double* B, int B_strideelems0, int B_strideelems1, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + { + if(jout + P_j0 < iout + P_i0) + { + // Wrong triangle. Set it up to look at the right triangle + // during the next loop interation + jout = iout + P_i0 - P_j0 - 1; + continue; + } + + for(int k=0; k<3; k++) + { + Psym66[index_sym66_assume_upper(iout+P_i0, jout+P_j0)] += + A[iout*A_strideelems0 + k *A_strideelems1] * + B[k *B_strideelems0 + jout*B_strideelems1] * scale; + } + } +} + +static +void set_gen33_from_gen33insym66(// output + double* restrict P, int P_strideelems0, int P_strideelems1, + // input + const double* Msym66, int M_i0, int M_j0, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + P[iout*P_strideelems0 + jout*P_strideelems1] = + Msym66[index_sym66(iout+M_i0, jout+M_j0)] * scale; +} +// Assumes the output is symmetric, and only computes the upper triangle + +static +void set_33insym66_from_gen33_accum(// output + double* restrict Psym66, int P_i0, int P_j0, + // input + const double* M, int M_strideelems0, int M_strideelems1, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + { + if(jout + P_j0 < iout + P_i0) + { + // Wrong triangle. Set it up to look at the right triangle + // during the next loop interation + jout = iout + P_i0 - P_j0 - 1; + continue; + } + + Psym66[index_sym66_assume_upper(iout+P_i0, jout+P_j0)] += + M[iout*M_strideelems0 + jout*M_strideelems1] * scale; + } +} + +// This is completely unreasonable. I'm almost certainly going to replace it + +static +double cofactors_sym6(const double* restrict m, double* restrict c) +{ + /* +Just like in libminimath; adding 6x6 version. I use the maxima result verbatim, except: + +- mNNN -> m[NNN] +- XXX^2 -> XXX*XXX + +maxima session: + +(%i1) display2d : false; + +(%i2) sym6 : matrix([m0,m1, m2, m3, m4, m5 ], + [m1,m6, m7, m8, m9, m10], + [m2,m7, m11, m12,m13, m14], + [m3,m8, m12, m15,m16, m17], + [m4,m9, m13, m16,m18, m19], + [m5,m10,m14, m17,m19, m20]); + +(%i10) num( ev(invert(sym6),detout) ); + +(%o10) matrix([(-m7*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7)) + +m8*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + -m9*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + +m10*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + +(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))) + *m6, + m2*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7) + -m3*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + +m4*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + -m5*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + -m1*(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + (-m2*((-m8*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m10*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m6)) + +m3*((-m7*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m6) + -m4*((-m7*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m8*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m6) + +m5*((-m7*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m8*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m9*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m6) + +m1*((m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m9 + -(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m8 + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7 + -m10*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + m2*((-m8*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m6) + -m3*((-m7*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + +(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m6) + +m4*((-m7*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7)) + +m8*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m6) + -m5*((-m7*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7)) + +m8*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -m9*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16)) + *m6) + -m1*((m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m9 + -(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m8 + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m7 + -m10*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))), + (-m2*(((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + *m9 + -m8*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m6)) + +m3*(((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + *m9 + -m7*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m6) + +m5*((-((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + *m9) + -m7*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +m8*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15)) + *m6) + +m1*((m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m9 + -(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m8 + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m7 + -m10*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + -m4*((-m7*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7)) + +m8*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -m10*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + +(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m6), + m2*((-m8*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -m10*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m6) + -m3*((-m7*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + +(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m6) + +m4*((-m7*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7)) + +m8*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m6) + -m5*((-m7*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7)) + +m8*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -m9*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)) + *m6) + -m1*((m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m9 + -(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m8 + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m7 + -m10*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)))], + [((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))) + *m9 + -((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + *m8 + +((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))) + *m7 + -m10*((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2) + -m1*(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + (-m2*((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)))) + +m3*((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + -m4*((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))) + +m5*((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2) + +m0*(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + m2*(((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m8 + -m10*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m1*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))) + -m3*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7 + -m10*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m1*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + -m5*((-((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m9) + +((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m8 + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7 + +m1*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))) + -m0*((m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m9 + -(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m8 + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7 + -m10*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))) + +m4*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m8 + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7 + -m10*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m1*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))), + (-m2*(((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m8 + -m10*((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + +m1*(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)))) + +m3*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m7 + -m10*((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + +m1*(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18))) + +m5*((-((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m9) + +((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m8 + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m7 + +m1*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))) + +m0*((m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m9 + -(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m8 + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m7 + -m10*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))) + -m4*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m8 + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16))), + m2*(((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m8 + -m10*((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + +m1*(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17))) + -m3*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16))) + -m5*((-((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m9) + +((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m8 + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m7 + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + -m0*((m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m9 + -(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m8 + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m7 + -m10*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + +m4*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m8 + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m7 + -m10*((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + +m1*(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15))), + (-m2*(((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m8 + -m10*((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + +m1*(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)))) + +m3*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m7 + -m10*((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + +m1*(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16))) + +m5*((-((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m9) + +((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m8 + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m7 + +m1*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15))) + +m0*((m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m9 + -(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m8 + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m7 + -m10*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15))) + -m4*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m8 + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m7 + -m10*((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)))], + [m8*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7) + -m9*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7) + +m10*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7) + +m1*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7) + -((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))) + *m6, + (-m3*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7)) + +m4*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7) + -m5*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7) + -m0*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7) + +m1*((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))), + m3*(m9*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m1*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m6) + -m4*(m8*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m6) + +m5*(m8*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m6) + +m0*((-m8*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m10*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m6) + -m1*(((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m8 + -m10*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m1*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))), + (-m3*(m9*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + +m1*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7) + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m6)) + +m4*(m8*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6) + -m5*(m8*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m6) + -m0*((-m8*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m6) + +m1*(((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m8 + -m10*((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + +m1*(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18))), + m3*((m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17)-(m20*m3-m17*m5)*m7) + *m9 + -m10*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6) + +m5*((-(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + *m9) + +m8*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m6) + +m0*(((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + *m9 + -m8*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m6) + -m1*(((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m8 + -m10*((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + +m1*(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17))) + -m4*(m8*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + +m1*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m6), + (-m3*(m9*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + +m1*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7) + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m6)) + +m4*(m8*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m6) + -m5*(m8*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m6) + -m0*((-m8*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -m10*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m6) + +m1*(((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m8 + -m10*((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + +m1*(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)))], + [(-m7*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7)) + +m9*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + -m10*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + -m1*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + +((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + *m6, + m2*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7) + -m4*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + +m5*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + +m0*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + -m1*((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))), + (-m2*(m9*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m1*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m6)) + +m4*(m7*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m6) + -m5*(m7*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m6) + -m0*((-m7*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m6) + +m1*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7 + -m10*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m1*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))), + m2*(m9*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + +m1*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7) + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m6) + -m4*(m7*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m6) + +m5*(m7*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m6) + +m0*((-m7*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + +(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m6) + -m1*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m7 + -m10*((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + +m1*(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18))), + (-m2*((m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + *m9 + -m10*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6)) + -m5*((-(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + *m9) + +m7*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + -((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m6) + -m0*(((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + *m9 + -m7*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m6) + +m1*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16))) + +m4*(m7*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m6), + m2*(m9*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + +m1*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7) + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m6) + -m4*(m7*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m6) + +m5*(m7*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m6) + +m0*((-m7*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + +(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m6) + -m1*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m7 + -m10*((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + +m1*(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)))], + [m7*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7) + -m8*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + +m10*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + +m1*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + -((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))) + *m6, + (-m2*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7)) + +m3*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + -m5*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + -m0*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + +m1*((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))), + m2*(m8*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m6) + -m3*(m7*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m6) + +m5*(m7*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m8*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m6) + +m0*((-m7*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m8*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m6) + -m1*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m8 + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7 + -m10*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m1*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))), + (-m2*(m8*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6)) + +m3*(m7*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m6) + -m5*(m7*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + -m8*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + -((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m6) + -m0*((-m7*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7)) + +m8*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m6) + +m1*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m8 + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16))), + m2*(m8*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + +m1*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m6) + -m3*(m7*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m6) + +m5*(m7*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + -m8*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + -((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m6) + +m0*((-m7*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7)) + +m8*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -m10*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + +(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m6) + -m1*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m8 + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m7 + -m10*((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + +m1*(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15))), + (-m2*(m8*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m6)) + +m3*(m7*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m6) + -m5*(m7*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + -m8*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + -((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m6) + -m0*((-m7*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7)) + +m8*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m6) + +m1*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m8 + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m7 + -m10*((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)))], + [(-m7*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7)) + +m8*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + -m9*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + -m1*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + +((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2) + *m6, + m2*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7) + -m3*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + +m4*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + +m0*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + -m1*((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2), + (-m2*(m8*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m6)) + +m3*(m7*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m6) + -m4*(m7*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m8*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m6) + -m0*((-m7*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m8*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m9*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m6) + +m1*((-((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m9) + +((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m8 + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7 + +m1*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + m2*(m8*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m6) + -m3*(m7*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m6) + +m4*(m7*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + -m8*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + -((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m6) + +m0*((-m7*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7)) + +m8*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -m9*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16)) + *m6) + -m1*((-((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m9) + +((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m8 + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m7 + +m1*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))), + (-m2*((-(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + *m9) + +m8*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m6)) + +m3*((-(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + *m9) + +m7*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + -((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m6) + -m0*((-((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + *m9) + -m7*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +m8*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15)) + *m6) + +m1*((-((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m9) + +((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m8 + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m7 + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + -m4*(m7*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + -m8*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + -((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m6), + m2*(m8*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m6) + -m3*(m7*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m6) + +m4*(m7*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + -m8*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + -((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m6) + +m0*((-m7*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7)) + +m8*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -m9*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)) + *m6) + -m1*((-((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m9) + +((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m8 + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m7 + +m1*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)))]) + +Test program: + + void main(void) + { + const double a[] = + { 1.9826929 , 0.49539104, 1.21536243, 0.98610923, 1.68623959, 1.0331091 , + 1.96643809, 1.42962549, 0.9336305 , 1.96542156, 0.6086516 , + 0.81542249, 0.74012536, 0.83940333, 1.58604071, + 0.1338364 , 1.03314221, 0.44817192, + 1.97146512, 0.27591278, + 1.51474051 }; + + double c[21]; + + int N=6; + double det = cofactors_sym6(a, c); + + printf("det = %f\n", det); + printf("\n\n"); + + double p[36] = {}; + + for(int iout=0; iout m.*m, m. -> m[.] + +(%i36) determinant(sym2); + +(%o36) m0*m2-m1^2 +(%i37) determinant(sym3); + +(%o37) m0*(m3*m5-m4^2)-m1*(m1*m5-m2*m4)+m2*(m1*m4-m2*m3) +(%i38) determinant(sym4); + +(%o38) m0*(m4*(m7*m9-m8^2)-m5*(m5*m9-m6*m8)+m6*(m5*m8-m6*m7)) + -m1*(m1*(m7*m9-m8^2)-m5*(m2*m9-m3*m8)+m6*(m2*m8-m3*m7)) + +m2*(m1*(m5*m9-m6*m8)-m4*(m2*m9-m3*m8)+m6*(m2*m6-m3*m5)) + -m3*(m1*(m5*m8-m6*m7)-m4*(m2*m8-m3*m7)+m5*(m2*m6-m3*m5)) + +(%i100) determinant(sym5); + +(%o100) -m3*(-m8*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m1*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)+(m10*m14-m11*m13)*m6) + -m5*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)+(m10*m14-m11*m13)*m2) + +m6*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6)) + +m4*(-m7*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m1*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + -m5*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m6*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6)) + +m0*(m7*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + -m8*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + +m5*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -m6*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6)) + -m1*(m7*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + -m8*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)+(m12*m14-m13^2)*m2) + *m6) + +m2*(m7*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6) + -m8*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6) + +m1*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)+(m12*m14-m13^2)*m6) + -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)+(m12*m14-m13^2)*m2) + *m5) + +*/ + +#if 0 +#error You really should be using the cofactors functions below to do this. So far Ive only needed the determinant as a part of computing the inverse, and the below functions do this much more efficiently +static inline double det_sym2(const double* m) +{ + return m[0]*m[2]-m[1]*m[1]; +} +static inline double det_sym3(const double* m) +{ + return m[0]*(m[3]*m[5]-m[4]*m[4])-m[1]*(m[1]*m[5]-m[2]*m[4])+m[2]*(m[1]*m[4]-m[2]*m[3]); +} +static inline double det_sym4(const double* m) +{ + return + +m[0]*(m[4]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[5]*m[9]-m[6]*m[8])+m[6]*(m[5]*m[8]-m[6]*m[7])) + -m[1]*(m[1]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[2]*m[9]-m[3]*m[8])+m[6]*(m[2]*m[8]-m[3]*m[7])) + +m[2]*(m[1]*(m[5]*m[9]-m[6]*m[8])-m[4]*(m[2]*m[9]-m[3]*m[8])+m[6]*(m[2]*m[6]-m[3]*m[5])) + -m[3]*(m[1]*(m[5]*m[8]-m[6]*m[7])-m[4]*(m[2]*m[8]-m[3]*m[7])+m[5]*(m[2]*m[6]-m[3]*m[5])); +} +static inline double det_sym5(const double* m) +{ + return + -m[3]*(-m[8]*((m[3]*m[8]-m[4]*m[7])*m[9]+m[2]*(m[11]*m[7]-m[10]*m[8])-(m[11]*m[3]-m[10]*m[4])*m[6]) + +m[1]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6]) + -m[5]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[14]-m[11]*m[13])*m[2]) + +m[6]*(m[11]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[6])) + +m[4]*(-m[7]*((m[3]*m[8]-m[4]*m[7])*m[9]+m[2]*(m[11]*m[7]-m[10]*m[8])-(m[11]*m[3]-m[10]*m[4])*m[6]) + +m[1]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8]) + +(m[10]*m[13]-m[11]*m[12])*m[6]) + -m[5]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4]) + +(m[10]*m[13]-m[11]*m[12])*m[2]) + +m[6]*(m[10]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[6])) + +m[0]*(m[7]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8]) + +(m[10]*m[14]-m[11]*m[13])*m[6]) + -m[8]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8]) + +(m[10]*m[13]-m[11]*m[12])*m[6]) + +m[5]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13]) + +m[11]*(m[10]*m[13]-m[11]*m[12])) + -m[6]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8]) + +(m[12]*m[14]-m[13]*m[13])*m[6])) + -m[1]*(m[7]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4]) + +(m[10]*m[14]-m[11]*m[13])*m[2]) + -m[8]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4]) + +(m[10]*m[13]-m[11]*m[12])*m[2]) + +m[1]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13]) + +m[11]*(m[10]*m[13]-m[11]*m[12])) + -(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2]) + *m[6]) + +m[2]*(m[7]*(m[11]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[6]) + -m[8]*(m[10]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[6]) + +m[1]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]) + -(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2]) + *m[5]); +} +#endif + +/* I now do inverses. I return transposed cofactors. Original matrix * cofactors / det = identity +As before, I replaced m.^2 -> m.*m, m. -> m[.] +I also removed the lower triangle, since I'm dealing with symmetric matrices here +Session: + + +(%i64) num( ev(invert(sym2),detout) ); + +(%o64) matrix([m2,-m1],[-m1,m0]) +(%i65) num( ev(invert(sym3),detout) ); + +(%o65) matrix([m3*m5-m4^2,m2*m4-m1*m5,m1*m4-m2*m3], + [m2*m4-m1*m5,m0*m5-m2^2,m1*m2-m0*m4], + [m1*m4-m2*m3,m1*m2-m0*m4,m0*m3-m1^2]) +(%i66) num( ev(invert(sym4),detout) ); + +(%o66) matrix([m4*(m7*m9-m8^2)-m5*(m5*m9-m6*m8)+m6*(m5*m8-m6*m7), + -m1*(m7*m9-m8^2)+m2*(m5*m9-m6*m8)-m3*(m5*m8-m6*m7), + m1*(m5*m9-m6*m8)-m2*(m4*m9-m6^2)+m3*(m4*m8-m5*m6), + -m1*(m5*m8-m6*m7)+m2*(m4*m8-m5*m6)-m3*(m4*m7-m5^2)], + [-m1*(m7*m9-m8^2)+m5*(m2*m9-m3*m8)-m6*(m2*m8-m3*m7), + m0*(m7*m9-m8^2)-m2*(m2*m9-m3*m8)+m3*(m2*m8-m3*m7), + -m0*(m5*m9-m6*m8)+m2*(m1*m9-m3*m6)-m3*(m1*m8-m3*m5), + m0*(m5*m8-m6*m7)-m2*(m1*m8-m2*m6)+m3*(m1*m7-m2*m5)], + [m1*(m5*m9-m6*m8)-m4*(m2*m9-m3*m8)+m6*(m2*m6-m3*m5), + -m0*(m5*m9-m6*m8)+m1*(m2*m9-m3*m8)-m3*(m2*m6-m3*m5), + m0*(m4*m9-m6^2)-m1*(m1*m9-m3*m6)+m3*(m1*m6-m3*m4), + -m0*(m4*m8-m5*m6)+m1*(m1*m8-m2*m6)-m3*(m1*m5-m2*m4)], + [-m1*(m5*m8-m6*m7)+m4*(m2*m8-m3*m7)-m5*(m2*m6-m3*m5), + m0*(m5*m8-m6*m7)-m1*(m2*m8-m3*m7)+m2*(m2*m6-m3*m5), + -m0*(m4*m8-m5*m6)+m1*(m1*m8-m3*m5)-m2*(m1*m6-m3*m4), + m0*(m4*m7-m5^2)-m1*(m1*m7-m2*m5)+m2*(m1*m5-m2*m4)]) +(%i103) num( ev(invert(sym5),detout) ); + +(%o103) matrix([m7*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + -m8*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + +m5*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -m6*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6), + -m3*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + +m4*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + -m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + +m2*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6), + -m2*(-m7*(m14*m7-m13*m8)+m8*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m5) + +m3*(-m6*(m14*m7-m13*m8)+m8*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m5) + -m4*(-m6*(m13*m7-m12*m8)+m7*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m5) + +m1*((m10*m13-m11*m12)*m8-(m10*m14-m11*m13)*m7 + +(m12*m14-m13^2)*m6), + -m3*(m8*(m11*m6-m8*m9)+m5*(m14*m9-m11^2)-m6*(m14*m6-m11*m8)) + +m4*(m7*(m11*m6-m8*m9)+m5*(m13*m9-m10*m11) + -m6*(m13*m6-m10*m8)) + -m1*(-m7*(m14*m9-m11^2)+m8*(m13*m9-m10*m11) + +(m10*m14-m11*m13)*m6) + +m2*(-m7*(m14*m6-m11*m8)+m8*(m13*m6-m10*m8) + +(m10*m14-m11*m13)*m5), + m3*(m8*(m10*m6-m7*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m11*m7)) + -m4*(m7*(m10*m6-m7*m9)+m5*(m12*m9-m10^2)-m6*(m12*m6-m10*m7)) + +m1*(-m7*(m13*m9-m10*m11)+m8*(m12*m9-m10^2) + +(m10*m13-m11*m12)*m6) + -m2*((m12*m6-m10*m7)*m8-m7*(m13*m6-m11*m7) + +(m10*m13-m11*m12)*m5)], + [-m7*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + +m8*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + -m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + +(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2) + *m6, + m3*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + -m4*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m0*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -m2*(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2), + m2*((m13*m3-m12*m4)*m8-(m14*m3-m13*m4)*m7+m1*(m12*m14-m13^2)) + -m3*((m11*m3-m10*m4)*m8-(m14*m3-m13*m4)*m6 + +m1*(m10*m14-m11*m13)) + -m0*((m10*m13-m11*m12)*m8-(m10*m14-m11*m13)*m7 + +(m12*m14-m13^2)*m6) + +m4*((m11*m3-m10*m4)*m7-(m13*m3-m12*m4)*m6 + +m1*(m10*m13-m11*m12)), + m3*(m8*(m11*m2-m4*m9)+m1*(m14*m9-m11^2)-(m14*m2-m11*m4)*m6) + -m4*(m7*(m11*m2-m4*m9)+m1*(m13*m9-m10*m11) + -(m13*m2-m10*m4)*m6) + +m0*(-m7*(m14*m9-m11^2)+m8*(m13*m9-m10*m11) + +(m10*m14-m11*m13)*m6) + -m2*((m13*m2-m10*m4)*m8-(m14*m2-m11*m4)*m7 + +m1*(m10*m14-m11*m13)), + -m3*(m8*(m10*m2-m3*m9)+m1*(m13*m9-m10*m11)-(m13*m2-m11*m3)*m6) + +m4*(m7*(m10*m2-m3*m9)+m1*(m12*m9-m10^2)-(m12*m2-m10*m3)*m6) + -m0*(-m7*(m13*m9-m10*m11)+m8*(m12*m9-m10^2) + +(m10*m13-m11*m12)*m6) + +m2*((m12*m2-m10*m3)*m8-(m13*m2-m11*m3)*m7 + +m1*(m10*m13-m11*m12))], + [m7*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6) + -m8*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6) + +m1*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6) + -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2) + *m5, + -m3*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6) + +m4*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6) + -m0*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6) + +m1*(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2), + m3*(m8*(m3*m8-m4*m7)+m1*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m5) + -m4*(m7*(m3*m8-m4*m7)+m1*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m5) + +m0*(-m7*(m14*m7-m13*m8)+m8*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m5) + -m1*((m13*m3-m12*m4)*m8-(m14*m3-m13*m4)*m7 + +m1*(m12*m14-m13^2)), + -m3*(m8*(m2*m8-m4*m6)+m1*(m14*m6-m11*m8)-(m14*m2-m11*m4)*m5) + +m4*(m7*(m2*m8-m4*m6)+m1*(m13*m6-m10*m8)-(m13*m2-m10*m4)*m5) + -m0*(-m7*(m14*m6-m11*m8)+m8*(m13*m6-m10*m8) + +(m10*m14-m11*m13)*m5) + +m1*((m13*m2-m10*m4)*m8-(m14*m2-m11*m4)*m7 + +m1*(m10*m14-m11*m13)), + m3*((m2*m7-m3*m6)*m8+m1*(m13*m6-m11*m7)-(m13*m2-m11*m3)*m5) + +m0*((m12*m6-m10*m7)*m8-m7*(m13*m6-m11*m7) + +(m10*m13-m11*m12)*m5) + -m1*((m12*m2-m10*m3)*m8-(m13*m2-m11*m3)*m7 + +m1*(m10*m13-m11*m12)) + -m4*(m7*(m2*m7-m3*m6)+m1*(m12*m6-m10*m7) + -(m12*m2-m10*m3)*m5)], + [m8*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + -m1*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + +m5*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + -m6*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8) + -(m14*m3-m13*m4)*m6), + -m4*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m0*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + -m1*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + +m2*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8) + -(m14*m3-m13*m4)*m6), + -m2*(m8*(m3*m8-m4*m7)+m1*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m5) + +m4*(m6*(m3*m8-m4*m7)+m1*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m5) + -m0*(-m6*(m14*m7-m13*m8)+m8*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m5) + +m1*((m11*m3-m10*m4)*m8-(m14*m3-m13*m4)*m6 + +m1*(m10*m14-m11*m13)), + m0*(m8*(m11*m6-m8*m9)+m5*(m14*m9-m11^2)-m6*(m14*m6-m11*m8)) + -m4*(m1*(m11*m6-m8*m9)-m5*(m11*m2-m4*m9)+m6*(m2*m8-m4*m6)) + -m1*(m8*(m11*m2-m4*m9)+m1*(m14*m9-m11^2)-(m14*m2-m11*m4)*m6) + +m2*(m8*(m2*m8-m4*m6)+m1*(m14*m6-m11*m8)-(m14*m2-m11*m4)*m5), + -m0*(m8*(m10*m6-m7*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m11*m7)) + +m4*(m1*(m10*m6-m7*m9)-m5*(m10*m2-m3*m9)+m6*(m2*m7-m3*m6)) + +m1*(m8*(m10*m2-m3*m9)+m1*(m13*m9-m10*m11) + -(m13*m2-m11*m3)*m6) + -m2*((m2*m7-m3*m6)*m8+m1*(m13*m6-m11*m7) + -(m13*m2-m11*m3)*m5)], + [-m7*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m1*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + -m5*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m6*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8) + -(m13*m3-m12*m4)*m6), + m3*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + -m0*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + +m1*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + -m2*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8) + -(m13*m3-m12*m4)*m6), + m2*(m7*(m3*m8-m4*m7)+m1*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m5) + -m3*(m6*(m3*m8-m4*m7)+m1*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m5) + +m0*(-m6*(m13*m7-m12*m8)+m7*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m5) + -m1*((m11*m3-m10*m4)*m7-(m13*m3-m12*m4)*m6 + +m1*(m10*m13-m11*m12)), + -m0*(m7*(m11*m6-m8*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m10*m8)) + +m3*(m1*(m11*m6-m8*m9)-m5*(m11*m2-m4*m9)+m6*(m2*m8-m4*m6)) + +m1*(m7*(m11*m2-m4*m9)+m1*(m13*m9-m10*m11) + -(m13*m2-m10*m4)*m6) + -m2*(m7*(m2*m8-m4*m6)+m1*(m13*m6-m10*m8)-(m13*m2-m10*m4)*m5), + m0*(m7*(m10*m6-m7*m9)+m5*(m12*m9-m10^2)-m6*(m12*m6-m10*m7)) + -m3*(m1*(m10*m6-m7*m9)-m5*(m10*m2-m3*m9)+m6*(m2*m7-m3*m6)) + -m1*(m7*(m10*m2-m3*m9)+m1*(m12*m9-m10^2)-(m12*m2-m10*m3)*m6) + +m2*(m7*(m2*m7-m3*m6)+m1*(m12*m6-m10*m7) + -(m12*m2-m10*m3)*m5)]) +*/ + +static inline double cofactors_sym2(const double* restrict m, double* restrict c) +{ + c[0] = m[2]; + c[1] = -m[1]; + c[2] = -m[1]; + + return m[0]*c[0] + m[1]*c[1]; +} + +static inline double cofactors_sym3(const double* restrict m, double* restrict c) +{ + c[0] = m[3]*m[5]-m[4]*m[4]; + c[1] = m[2]*m[4]-m[1]*m[5]; + c[2] = m[1]*m[4]-m[2]*m[3]; + c[3] = m[0]*m[5]-m[2]*m[2]; + c[4] = m[1]*m[2]-m[0]*m[4]; + c[5] = m[0]*m[3]-m[1]*m[1]; + + return m[0]*c[0] + m[1]*c[1] + m[2]*c[2]; +} + +static inline double cofactors_sym4(const double* restrict m, double* restrict c) +{ + c[0] = m[4]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[5]*m[9]-m[6]*m[8])+m[6]*(m[5]*m[8]-m[6]*m[7]); + c[1] = -m[1]*(m[7]*m[9]-m[8]*m[8])+m[2]*(m[5]*m[9]-m[6]*m[8])-m[3]*(m[5]*m[8]-m[6]*m[7]); + c[2] = m[1]*(m[5]*m[9]-m[6]*m[8])-m[2]*(m[4]*m[9]-m[6]*m[6])+m[3]*(m[4]*m[8]-m[5]*m[6]); + c[3] = -m[1]*(m[5]*m[8]-m[6]*m[7])+m[2]*(m[4]*m[8]-m[5]*m[6])-m[3]*(m[4]*m[7]-m[5]*m[5]); + c[4] = m[0]*(m[7]*m[9]-m[8]*m[8])-m[2]*(m[2]*m[9]-m[3]*m[8])+m[3]*(m[2]*m[8]-m[3]*m[7]); + c[5] = -m[0]*(m[5]*m[9]-m[6]*m[8])+m[2]*(m[1]*m[9]-m[3]*m[6])-m[3]*(m[1]*m[8]-m[3]*m[5]); + c[6] = m[0]*(m[5]*m[8]-m[6]*m[7])-m[2]*(m[1]*m[8]-m[2]*m[6])+m[3]*(m[1]*m[7]-m[2]*m[5]); + c[7] = m[0]*(m[4]*m[9]-m[6]*m[6])-m[1]*(m[1]*m[9]-m[3]*m[6])+m[3]*(m[1]*m[6]-m[3]*m[4]); + c[8] = -m[0]*(m[4]*m[8]-m[5]*m[6])+m[1]*(m[1]*m[8]-m[2]*m[6])-m[3]*(m[1]*m[5]-m[2]*m[4]); + c[9] = m[0]*(m[4]*m[7]-m[5]*m[5])-m[1]*(m[1]*m[7]-m[2]*m[5])+m[2]*(m[1]*m[5]-m[2]*m[4]); + + return m[0]*c[0] + m[1]*c[1] + m[2]*c[2] + m[3]*c[3]; +} + +static inline double cofactors_sym5(const double* restrict m, double* restrict c) +{ + c[0] = m[7]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6])-m[8]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[6])+m[5]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))-m[6]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]); + c[1] = -m[3]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6])+m[4]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[6])-m[1]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))+m[2]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]); + c[2] = -m[2]*(-m[7]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[5])+m[3]*(-m[6]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5])-m[4]*(-m[6]*(m[13]*m[7]-m[12]*m[8])+m[7]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[5])+m[1]*((m[10]*m[13]-m[11]*m[12])*m[8]-(m[10]*m[14]-m[11]*m[13])*m[7]+(m[12]*m[14]-m[13]*m[13])*m[6]); + c[3] = -m[3]*(m[8]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[14]*m[9]-m[11]*m[11])-m[6]*(m[14]*m[6]-m[11]*m[8]))+m[4]*(m[7]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[10]*m[8]))-m[1]*(-m[7]*(m[14]*m[9]-m[11]*m[11])+m[8]*(m[13]*m[9]-m[10]*m[11])+(m[10]*m[14]-m[11]*m[13])*m[6])+m[2]*(-m[7]*(m[14]*m[6]-m[11]*m[8])+m[8]*(m[13]*m[6]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5]); + c[4] = m[3]*(m[8]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[11]*m[7]))-m[4]*(m[7]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[12]*m[9]-m[10]*m[10])-m[6]*(m[12]*m[6]-m[10]*m[7]))+m[1]*(-m[7]*(m[13]*m[9]-m[10]*m[11])+m[8]*(m[12]*m[9]-m[10]*m[10])+(m[10]*m[13]-m[11]*m[12])*m[6])-m[2]*((m[12]*m[6]-m[10]*m[7])*m[8]-m[7]*(m[13]*m[6]-m[11]*m[7])+(m[10]*m[13]-m[11]*m[12])*m[5]); + c[5] = m[3]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[14]-m[11]*m[13])*m[2])-m[4]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[13]-m[11]*m[12])*m[2])+m[0]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))-m[2]*(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2]); + c[6] = m[2]*((m[13]*m[3]-m[12]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[7]+m[1]*(m[12]*m[14]-m[13]*m[13]))-m[3]*((m[11]*m[3]-m[10]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[6]+m[1]*(m[10]*m[14]-m[11]*m[13]))-m[0]*((m[10]*m[13]-m[11]*m[12])*m[8]-(m[10]*m[14]-m[11]*m[13])*m[7]+(m[12]*m[14]-m[13]*m[13])*m[6])+m[4]*((m[11]*m[3]-m[10]*m[4])*m[7]-(m[13]*m[3]-m[12]*m[4])*m[6]+m[1]*(m[10]*m[13]-m[11]*m[12])); + c[7] = m[3]*(m[8]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[14]*m[9]-m[11]*m[11])-(m[14]*m[2]-m[11]*m[4])*m[6])-m[4]*(m[7]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[10]*m[4])*m[6])+m[0]*(-m[7]*(m[14]*m[9]-m[11]*m[11])+m[8]*(m[13]*m[9]-m[10]*m[11])+(m[10]*m[14]-m[11]*m[13])*m[6])-m[2]*((m[13]*m[2]-m[10]*m[4])*m[8]-(m[14]*m[2]-m[11]*m[4])*m[7]+m[1]*(m[10]*m[14]-m[11]*m[13])); + c[8] = -m[3]*(m[8]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[11]*m[3])*m[6])+m[4]*(m[7]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[12]*m[9]-m[10]*m[10])-(m[12]*m[2]-m[10]*m[3])*m[6])-m[0]*(-m[7]*(m[13]*m[9]-m[10]*m[11])+m[8]*(m[12]*m[9]-m[10]*m[10])+(m[10]*m[13]-m[11]*m[12])*m[6])+m[2]*((m[12]*m[2]-m[10]*m[3])*m[8]-(m[13]*m[2]-m[11]*m[3])*m[7]+m[1]*(m[10]*m[13]-m[11]*m[12])); + c[9] = m[3]*(m[8]*(m[3]*m[8]-m[4]*m[7])+m[1]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[5])-m[4]*(m[7]*(m[3]*m[8]-m[4]*m[7])+m[1]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[5])+m[0]*(-m[7]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[5])-m[1]*((m[13]*m[3]-m[12]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[7]+m[1]*(m[12]*m[14]-m[13]*m[13])); + c[10] = -m[3]*(m[8]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[14]*m[6]-m[11]*m[8])-(m[14]*m[2]-m[11]*m[4])*m[5])+m[4]*(m[7]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[13]*m[6]-m[10]*m[8])-(m[13]*m[2]-m[10]*m[4])*m[5])-m[0]*(-m[7]*(m[14]*m[6]-m[11]*m[8])+m[8]*(m[13]*m[6]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5])+m[1]*((m[13]*m[2]-m[10]*m[4])*m[8]-(m[14]*m[2]-m[11]*m[4])*m[7]+m[1]*(m[10]*m[14]-m[11]*m[13])); + c[11] = m[3]*((m[2]*m[7]-m[3]*m[6])*m[8]+m[1]*(m[13]*m[6]-m[11]*m[7])-(m[13]*m[2]-m[11]*m[3])*m[5])+m[0]*((m[12]*m[6]-m[10]*m[7])*m[8]-m[7]*(m[13]*m[6]-m[11]*m[7])+(m[10]*m[13]-m[11]*m[12])*m[5])-m[1]*((m[12]*m[2]-m[10]*m[3])*m[8]-(m[13]*m[2]-m[11]*m[3])*m[7]+m[1]*(m[10]*m[13]-m[11]*m[12]))-m[4]*(m[7]*(m[2]*m[7]-m[3]*m[6])+m[1]*(m[12]*m[6]-m[10]*m[7])-(m[12]*m[2]-m[10]*m[3])*m[5]); + c[12] = m[0]*(m[8]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[14]*m[9]-m[11]*m[11])-m[6]*(m[14]*m[6]-m[11]*m[8]))-m[4]*(m[1]*(m[11]*m[6]-m[8]*m[9])-m[5]*(m[11]*m[2]-m[4]*m[9])+m[6]*(m[2]*m[8]-m[4]*m[6]))-m[1]*(m[8]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[14]*m[9]-m[11]*m[11])-(m[14]*m[2]-m[11]*m[4])*m[6])+m[2]*(m[8]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[14]*m[6]-m[11]*m[8])-(m[14]*m[2]-m[11]*m[4])*m[5]); + c[13] = -m[0]*(m[8]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[11]*m[7]))+m[4]*(m[1]*(m[10]*m[6]-m[7]*m[9])-m[5]*(m[10]*m[2]-m[3]*m[9])+m[6]*(m[2]*m[7]-m[3]*m[6]))+m[1]*(m[8]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[11]*m[3])*m[6])-m[2]*((m[2]*m[7]-m[3]*m[6])*m[8]+m[1]*(m[13]*m[6]-m[11]*m[7])-(m[13]*m[2]-m[11]*m[3])*m[5]); + c[14] = m[0]*(m[7]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[12]*m[9]-m[10]*m[10])-m[6]*(m[12]*m[6]-m[10]*m[7]))-m[3]*(m[1]*(m[10]*m[6]-m[7]*m[9])-m[5]*(m[10]*m[2]-m[3]*m[9])+m[6]*(m[2]*m[7]-m[3]*m[6]))-m[1]*(m[7]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[12]*m[9]-m[10]*m[10])-(m[12]*m[2]-m[10]*m[3])*m[6])+m[2]*(m[7]*(m[2]*m[7]-m[3]*m[6])+m[1]*(m[12]*m[6]-m[10]*m[7])-(m[12]*m[2]-m[10]*m[3])*m[5]); + + return m[0]*c[0] + m[1]*c[1] + m[2]*c[2] + m[3]*c[3] + m[4]*c[4]; +} + +/* +The upper-triangular and lower-triangular routines have a similar API to the +symmetric ones. Note that as with symmetric matrices, we don't store redundant +data, and we store data row-first. So the upper-triangular matrices have N +elements in the first row in memory, but lower-triangular matrices have only 1. + +Inverses of triangular matrices are similarly triangular, and that's how I store +them + + +Session: + +// upper triangular +(%i2) ut2 : matrix([m0,m1],[0,m2]); + +(%o2) matrix([m0,m1],[0,m2]) +(%i3) ut3 : matrix([m0,m1,m2],[0,m3,m4],[0,0,m5]); + +(%o3) matrix([m0,m1,m2],[0,m3,m4],[0,0,m5]) +(%i4) ut4 : matrix([m0,m1,m2,m3],[0,m4,m5,m6],[0,0,m7,m8],[0,0,0,m9]); + +(%o4) matrix([m0,m1,m2,m3],[0,m4,m5,m6],[0,0,m7,m8],[0,0,0,m9]) +(%i5) ut5 : matrix([m0,m1,m2,m3,m4],[0,m5,m6,m7,m8],[0,0,m9,m10,m11],[0,0,0,m12,m13],[0,0,0,0,m14]); + +(%o5) matrix([m0,m1,m2,m3,m4],[0,m5,m6,m7,m8],[0,0,m9,m10,m11], + [0,0,0,m12,m13],[0,0,0,0,m14]) + +(%i11) num( ev(invert(ut2),detout) ); + +(%o11) matrix([m2,-m1],[0,m0]) +(%i12) num( ev(invert(ut3),detout) ); + +(%o12) matrix([m3*m5,-m1*m5,m1*m4-m2*m3],[0,m0*m5,-m0*m4],[0,0,m0*m3]) +(%i13) num( ev(invert(ut4),detout) ); + +(%o13) matrix([m4*m7*m9,-m1*m7*m9,m1*m5*m9-m2*m4*m9, + (-m1*(m5*m8-m6*m7))+m2*m4*m8-m3*m4*m7], + [0,m0*m7*m9,-m0*m5*m9,m0*(m5*m8-m6*m7)], + [0,0,m0*m4*m9,-m0*m4*m8],[0,0,0,m0*m4*m7]) +(%i14) num( ev(invert(ut5),detout) ); + +(%o14) matrix([m12*m14*m5*m9,-m1*m12*m14*m9,m1*m12*m14*m6-m12*m14*m2*m5, + (-m1*(m10*m14*m6-m14*m7*m9))-m14*m3*m5*m9+m10*m14*m2*m5, + m1*(m12*m8*m9-m13*m7*m9+(m10*m13-m11*m12)*m6) + -m12*m4*m5*m9+m13*m3*m5*m9-(m10*m13-m11*m12)*m2*m5], + [0,m0*m12*m14*m9,-m0*m12*m14*m6,m0*(m10*m14*m6-m14*m7*m9), + -m0*(m12*m8*m9-m13*m7*m9+(m10*m13-m11*m12)*m6)], + [0,0,m0*m12*m14*m5,-m0*m10*m14*m5,m0*(m10*m13-m11*m12)*m5], + [0,0,0,m0*m14*m5*m9,-m0*m13*m5*m9],[0,0,0,0,m0*m12*m5*m9]) + + +// lower-triangular +(%i19) lt2 : matrix([m0,0],[m1,m2]); + +(%o19) matrix([m0,0],[m1,m2]) +(%i20) lt3 : matrix([m0,0,0],[m1,m2,0],[m3,m4,m5]); + +(%o20) matrix([m0,0,0],[m1,m2,0],[m3,m4,m5]) +(%i21) lt4 : matrix([m0,0,0,0],[m1,m2,0,0],[m3,m4,m5,0],[m6,m7,m8,m9]); + +(%o21) matrix([m0,0,0,0],[m1,m2,0,0],[m3,m4,m5,0],[m6,m7,m8,m9]) +(%i22) lt5 : matrix([m0,0,0,0,0],[m1,m2,0,0,0],[m3,m4,m5,0,0],[m6,m7,m8,m9,0],[m10,m11,m12,m13,m14]); + +(%o22) matrix([m0,0,0,0,0],[m1,m2,0,0,0],[m3,m4,m5,0,0],[m6,m7,m8,m9,0], + [m10,m11,m12,m13,m14]) +(%i23) num( ev(invert(lt2),detout) ); + +(%o23) matrix([m2,0],[-m1,m0]) +(%i24) num( ev(invert(lt3),detout) ); + +(%o24) matrix([m2*m5,0,0],[-m1*m5,m0*m5,0],[m1*m4-m2*m3,-m0*m4,m0*m2]) +(%i25) num( ev(invert(lt4),detout) ); + +(%o25) matrix([m2*m5*m9,0,0,0],[-m1*m5*m9,m0*m5*m9,0,0], + [m1*m4*m9-m2*m3*m9,-m0*m4*m9,m0*m2*m9,0], + [m2*(m3*m8-m5*m6)-m1*(m4*m8-m5*m7),m0*(m4*m8-m5*m7),-m0*m2*m8, + m0*m2*m5]) +(%i26) num( ev(invert(lt5),detout) ); + +(%o26) matrix([m14*m2*m5*m9,0,0,0,0],[-m1*m14*m5*m9,m0*m14*m5*m9,0,0,0], + [m1*m14*m4*m9-m14*m2*m3*m9,-m0*m14*m4*m9,m0*m14*m2*m9,0,0], + [m2*(m14*m3*m8-m14*m5*m6)-m1*(m14*m4*m8-m14*m5*m7), + m0*(m14*m4*m8-m14*m5*m7),-m0*m14*m2*m8,m0*m14*m2*m5,0], + [m1*(m4*(m13*m8-m12*m9)-m5*(m13*m7-m11*m9)) + -m2*(m3*(m13*m8-m12*m9)-m5*(m13*m6-m10*m9)), + -m0*(m4*(m13*m8-m12*m9)-m5*(m13*m7-m11*m9)), + m0*m2*(m13*m8-m12*m9),-m0*m13*m2*m5,m0*m2*m5*m9]) + + */ +static inline double cofactors_ut2(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]; + c[i++] = -m[1]; + c[i++] = m[0]; + return m[0]*m[2]; +} +static inline double cofactors_ut3(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[3]*m[5]; + c[i++] = -m[1]*m[5]; + c[i++] = m[1]*m[4]-m[2]*m[3]; + c[i++] = m[0]*m[5]; + c[i++] = -m[0]*m[4]; + c[i++] = m[0]*m[3]; + return m[0]*m[3]*m[5]; +} +static inline double cofactors_ut4(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[4]*m[7]*m[9]; + c[i++] = -m[1]*m[7]*m[9]; + c[i++] = m[1]*m[5]*m[9]-m[2]*m[4]*m[9]; + c[i++] = (-m[1]*(m[5]*m[8]-m[6]*m[7]))+m[2]*m[4]*m[8]-m[3]*m[4]*m[7]; + c[i++] = m[0]*m[7]*m[9]; + c[i++] = -m[0]*m[5]*m[9]; + c[i++] = m[0]*(m[5]*m[8]-m[6]*m[7]); + c[i++] = m[0]*m[4]*m[9]; + c[i++] = -m[0]*m[4]*m[8]; + c[i++] = m[0]*m[4]*m[7]; + return m[0]*m[4]*m[7]*m[9]; +} +static inline double cofactors_ut5(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[12]*m[14]*m[5]*m[9]; + c[i++] = -m[1]*m[12]*m[14]*m[9]; + c[i++] = m[1]*m[12]*m[14]*m[6]-m[12]*m[14]*m[2]*m[5]; + c[i++] = (-m[1]*(m[10]*m[14]*m[6]-m[14]*m[7]*m[9]))-m[14]*m[3]*m[5]*m[9]+m[10]*m[14]*m[2]*m[5]; + c[i++] = m[1]*(m[12]*m[8]*m[9]-m[13]*m[7]*m[9]+(m[10]*m[13]-m[11]*m[12])*m[6])-m[12]*m[4]*m[5]*m[9]+m[13]*m[3]*m[5]*m[9]-(m[10]*m[13]-m[11]*m[12])*m[2]*m[5]; + c[i++] = m[0]*m[12]*m[14]*m[9]; + c[i++] = -m[0]*m[12]*m[14]*m[6]; + c[i++] = m[0]*(m[10]*m[14]*m[6]-m[14]*m[7]*m[9]); + c[i++] = -m[0]*(m[12]*m[8]*m[9]-m[13]*m[7]*m[9]+(m[10]*m[13]-m[11]*m[12])*m[6]); + c[i++] = m[0]*m[12]*m[14]*m[5]; + c[i++] = -m[0]*m[10]*m[14]*m[5]; + c[i++] = m[0]*(m[10]*m[13]-m[11]*m[12])*m[5]; + c[i++] = m[0]*m[14]*m[5]*m[9]; + c[i++] = -m[0]*m[13]*m[5]*m[9]; + c[i++] = m[0]*m[12]*m[5]*m[9]; + return m[0]*m[5]*m[9]*m[12]*m[14]; +} +static inline double cofactors_lt2(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]; + c[i++] = -m[1]; + c[i++] = m[0]; + return m[0]*m[2]; +} +static inline double cofactors_lt3(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]*m[5]; + c[i++] = -m[1]*m[5]; + c[i++] = m[0]*m[5]; + c[i++] = m[1]*m[4]-m[2]*m[3]; + c[i++] = -m[0]*m[4]; + c[i++] = m[0]*m[2]; + return m[0]*m[2]*m[5]; +} +static inline double cofactors_lt4(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]*m[5]*m[9]; + c[i++] = -m[1]*m[5]*m[9]; + c[i++] = m[0]*m[5]*m[9]; + c[i++] = m[1]*m[4]*m[9]-m[2]*m[3]*m[9]; + c[i++] = -m[0]*m[4]*m[9]; + c[i++] = m[0]*m[2]*m[9]; + c[i++] = m[2]*(m[3]*m[8]-m[5]*m[6])-m[1]*(m[4]*m[8]-m[5]*m[7]); + c[i++] = m[0]*(m[4]*m[8]-m[5]*m[7]); + c[i++] = -m[0]*m[2]*m[8]; + c[i++] = m[0]*m[2]*m[5]; + return m[0]*m[2]*m[5]*m[9]; +} +static inline double cofactors_lt5(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[14]*m[2]*m[5]*m[9]; + c[i++] = -m[1]*m[14]*m[5]*m[9]; + c[i++] = m[0]*m[14]*m[5]*m[9]; + c[i++] = m[1]*m[14]*m[4]*m[9]-m[14]*m[2]*m[3]*m[9]; + c[i++] = -m[0]*m[14]*m[4]*m[9]; + c[i++] = m[0]*m[14]*m[2]*m[9]; + c[i++] = m[2]*(m[14]*m[3]*m[8]-m[14]*m[5]*m[6])-m[1]*(m[14]*m[4]*m[8]-m[14]*m[5]*m[7]); + c[i++] = m[0]*(m[14]*m[4]*m[8]-m[14]*m[5]*m[7]); + c[i++] = -m[0]*m[14]*m[2]*m[8]; + c[i++] = m[0]*m[14]*m[2]*m[5]; + c[i++] = m[1]*(m[4]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[7]-m[11]*m[9]))-m[2]*(m[3]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[6]-m[10]*m[9])); + c[i++] = -m[0]*(m[4]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[7]-m[11]*m[9])); + c[i++] = m[0]*m[2]*(m[13]*m[8]-m[12]*m[9]); + c[i++] = -m[0]*m[13]*m[2]*m[5]; + c[i++] = m[0]*m[2]*m[5]*m[9]; + return m[0]*m[2]*m[5]*m[9]*m[14]; +} + +/* +(%i27) a : matrix([a0,a1,a2],[0,a3,a4],[0,0,a5]); + +(%o27) matrix([a0,a1,a2],[0,a3,a4],[0,0,a5]) +(%i28) b : matrix([b0,b1,b2],[0,b3,b4],[0,0,b5]); + +(%o28) matrix([b0,b1,b2],[0,b3,b4],[0,0,b5]) +(%i29) a . b; + +(%o29) matrix([a0*b0,a1*b3+a0*b1,a2*b5+a1*b4+a0*b2],[0,a3*b3,a4*b5+a3*b4],[0,0,a5*b5]) +*/ +static inline void mul_ut3_ut3(const double* restrict a, const double* restrict b, + double* restrict ab) +{ + ab[0] = a[0] * b[0]; + ab[1] = a[1] * b[3]+a[0] * b[1]; + ab[2] = a[2] * b[5]+a[1] * b[4]+a[0] * b[2]; + ab[3] = a[3] * b[3]; + ab[4] = a[4] * b[5]+a[3] * b[4]; + ab[5] = a[5] * b[5]; +} + +// symmetrix 3x3 by symmetrix 3x3, written into a new non-symmetric matrix, +// scaled. This is a special-case function that I needed for something... +static inline void mul_sym33_sym33_scaled_out(const double* restrict s0, const double* restrict s1, double* restrict mout, double scale) +{ +// (%i106) matrix([m0_0,m0_1,m0_2], +// [m0_1,m0_3,m0_4], +// [m0_2,m0_4,m0_5]) . +// matrix([m1_0,m1_1,m1_2], +// [m1_1,m1_3,m1_4], +// [m1_2,m1_4,m1_5]); + +// (%o106) matrix([m0_2*m1_2+m0_1*m1_1+m0_0*m1_0,m0_2*m1_4+m0_1*m1_3+m0_0*m1_1, +// m0_2*m1_5+m0_1*m1_4+m0_0*m1_2], +// [m0_4*m1_2+m0_3*m1_1+m0_1*m1_0,m0_4*m1_4+m0_3*m1_3+m0_1*m1_1, +// m0_4*m1_5+m0_3*m1_4+m0_1*m1_2], +// [m0_5*m1_2+m0_4*m1_1+m0_2*m1_0,m0_5*m1_4+m0_4*m1_3+m0_2*m1_1, +// m0_5*m1_5+m0_4*m1_4+m0_2*m1_2]) + + mout[0] = scale * (s0[2]*s1[2]+s0[1]*s1[1]+s0[0]*s1[0]); + mout[1] = scale * (s0[2]*s1[4]+s0[1]*s1[3]+s0[0]*s1[1]); + mout[2] = scale * (s0[2]*s1[5]+s0[1]*s1[4]+s0[0]*s1[2]); + mout[3] = scale * (s0[4]*s1[2]+s0[3]*s1[1]+s0[1]*s1[0]); + mout[4] = scale * (s0[4]*s1[4]+s0[3]*s1[3]+s0[1]*s1[1]); + mout[5] = scale * (s0[4]*s1[5]+s0[3]*s1[4]+s0[1]*s1[2]); + mout[6] = scale * (s0[5]*s1[2]+s0[4]*s1[1]+s0[2]*s1[0]); + mout[7] = scale * (s0[5]*s1[4]+s0[4]*s1[3]+s0[2]*s1[1]); + mout[8] = scale * (s0[5]*s1[5]+s0[4]*s1[4]+s0[2]*s1[2]); +} + +static inline void outerproduct3(const double* restrict v, double* restrict P) +{ + P[0] = v[0]*v[0]; + P[1] = v[0]*v[1]; + P[2] = v[0]*v[2]; + P[3] = v[1]*v[1]; + P[4] = v[1]*v[2]; + P[5] = v[2]*v[2]; +} + +static inline void outerproduct3_scaled(const double* restrict v, double* restrict P, double scale) +{ + P[0] = scale * v[0]*v[0]; + P[1] = scale * v[0]*v[1]; + P[2] = scale * v[0]*v[2]; + P[3] = scale * v[1]*v[1]; + P[4] = scale * v[1]*v[2]; + P[5] = scale * v[2]*v[2]; +} + +// conjugate 2 vectors (a, b) through a symmetric matrix S: a->transpose x S x b +// (%i2) sym3 : matrix([s0,s1,s2], +// [s1,s3,s4], +// [s2,s4,s5]); + +// (%o2) matrix([s0,s1,s2],[s1,s3,s4],[s2,s4,s5]) +// (%i6) a : matrix([a0],[a1],[a2]); + +// (%o6) matrix([a0],[a1],[a2]) +// (%i7) b : matrix([b0],[b1],[b2]); + +// (%o7) matrix([b0],[b1],[b2]) +// (%i10) transpose(a) . sym3 . b; + +// (%o10) a2*(b2*s5+b1*s4+b0*s2)+a1*(b2*s4+b1*s3+b0*s1)+a0*(b2*s2+b1*s1+b0*s0) +static inline double conj_3(const double* restrict a, const double* restrict s, const double* restrict b) +{ + return a[2]*(b[2]*s[5]+b[1]*s[4]+b[0]*s[2])+a[1]*(b[2]*s[4]+b[1]*s[3]+b[0]*s[1])+a[0]*(b[2]*s[2]+b[1]*s[1]+b[0]*s[0]); +} + +// Given an orthonormal matrix, returns the det. This is always +1 or -1 +static inline double det_orthonormal33(const double* m) +{ + // cross(row0,row1) = det * row3 + + // I find a nice non-zero element of row3, and see if the signs match + if( m[6] < -0.1 ) + { + // looking at col0 of the last row. It is <0 + double cross = m[1]*m[5] - m[2]*m[4]; + return cross > 0.0 ? -1.0 : 1.0; + } + if( m[6] > 0.1) + { + // looking at col0 of the last row. It is > 0 + double cross = m[1]*m[5] - m[2]*m[4]; + return cross > 0.0 ? 1.0 : -1.0; + } + + if( m[7] < -0.1 ) + { + // looking at col1 of the last row. It is <0 + double cross = m[2]*m[3] - m[0]*m[5]; + return cross > 0.0 ? -1.0 : 1.0; + } + if( m[7] > 0.1) + { + // looking at col1 of the last row. It is > 0 + double cross = m[2]*m[3] - m[0]*m[5]; + return cross > 0.0 ? 1.0 : -1.0; + } + + if( m[8] < -0.1 ) + { + // looking at col2 of the last row. It is <0 + double cross = m[0]*m[4] - m[1]*m[3]; + return cross > 0.0 ? -1.0 : 1.0; + } + + // last option. This MUST be true, so I don't even bother to check + { + // looking at col2 of the last row. It is > 0 + double cross = m[0]*m[4] - m[1]*m[3]; + return cross > 0.0 ? 1.0 : -1.0; + } +} + +static void minimath_xchg(double* m, int i, int j) +{ + double t = m[i]; + m[i] = m[j]; + m[j] = t; +} +static inline void gen33_transpose(double* m) +{ + minimath_xchg(m, 1, 3); + minimath_xchg(m, 2, 6); + minimath_xchg(m, 5, 7); +} + +static inline void gen33_transpose_vout(const double* m, double* mout) +{ + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + mout[i*3+j] = m[j*3+i]; +} + +static inline double cofactors_gen33(// output + double* restrict c, + + // input + const double* restrict m) +{ + /* +(%i1) display2d : false; + +(%o1) false +(%i5) linel : 100000; + +(%o5) 100000 +(%i6) mat33 : matrix( [m0,m1,m2], [m3,m4,m5], [m6,m7,m8] ); + +(%o6) matrix([m0,m1,m2],[m3,m4,m5],[m6,m7,m8]) +(%i7) num( ev(invert(mat33)), detout ); + +(%o7) matrix([(m4*m8-m5*m7)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m2*m7-m1*m8)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m1*m5-m2*m4)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))],[(m5*m6-m3*m8)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m0*m8-m2*m6)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m2*m3-m0*m5)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))],[(m3*m7-m4*m6)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m1*m6-m0*m7)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m0*m4-m1*m3)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))]) +(%i8) determinant(mat33); + +(%o8) m0*(m4*m8-m5*m7)-m1*(m3*m8-m5*m6)+m2*(m3*m7-m4*m6) + */ + + double det = m[0]*(m[4]*m[8]-m[5]*m[7])-m[1]*(m[3]*m[8]-m[5]*m[6])+m[2]*(m[3]*m[7]-m[4]*m[6]); + + c[0] = m[4]*m[8]-m[5]*m[7]; + c[1] = m[2]*m[7]-m[1]*m[8]; + c[2] = m[1]*m[5]-m[2]*m[4]; + c[3] = m[5]*m[6]-m[3]*m[8]; + c[4] = m[0]*m[8]-m[2]*m[6]; + c[5] = m[2]*m[3]-m[0]*m[5]; + c[6] = m[3]*m[7]-m[4]*m[6]; + c[7] = m[1]*m[6]-m[0]*m[7]; + c[8] = m[0]*m[4]-m[1]*m[3]; + + return det; +} + +#ifdef __cplusplus +#undef restrict +#endif diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h new file mode 100644 index 0000000000..086810d4c6 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h @@ -0,0 +1,124 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +// mrcal images. These are completely uninteresting, and don't do anything +// better that other image read/write APIS. If you have image libraries running, +// use those. If not, the ones defined here should be light and painless + +// I support several image types: +// - "uint8": 8-bit grayscale +// - "uint16": 16-bit grayscale (using the system endian-ness) +// - "bgr": 24-bit BGR color +// +// Each type defines several functions in the MRCAL_IMAGE_DECLARE() macro: +// +// - mrcal_image_TYPE_t container image +// - mrcal_image_TYPE_at(mrcal_image_TYPE_t* image, int x, int y) +// - mrcal_image_TYPE_at_const(const mrcal_image_TYPE_t* image, int x, int y) +// - mrcal_image_TYPE_t mrcal_image_TYPE_crop(mrcal_image_TYPE_t* image, in x0, int y0, int w, int h) +// - mrcal_image_TYPE_save (const char* filename, const mrcal_image_TYPE_t* image); +// - mrcal_image_TYPE_load( mrcal_image_TYPE_t* image, const char* filename); +// +// The image-loading functions require a few notes: +// +// An image structure to fill in is given. image->data will be allocated to the +// proper size. It is the caller's responsibility to free(image->data) when +// they're done. Usage sample: +// +// mrcal_image_uint8_t image; +// mrcal_image_uint8_load(&image, image_filename); +// .... do stuff ... +// free(image.data); +// +// mrcal_image_uint8_load() converts images to 8-bpp grayscale. Color and +// palettized images are accepted +// +// mrcal_image_uint16_load() does NOT convert images. The images being read must +// already be stored as 16bpp grayscale images +// +// mrcal_image_bgr_load() converts images to 24-bpp color + +#include +#include + + +typedef struct { uint8_t bgr[3]; } mrcal_bgr_t; + +#define MRCAL_IMAGE_DECLARE(T, Tname) \ +typedef struct \ +{ \ + union \ + { \ + /* in pixels */ \ + struct {int w, h;}; \ + struct {int width, height;}; \ + struct {int cols, rows;}; \ + }; \ + int stride; /* in bytes */ \ + T* data; \ +} mrcal_image_ ## Tname ## _t; \ + \ +static inline \ +T* mrcal_image_ ## Tname ## _at(mrcal_image_ ## Tname ## _t* image, int x, int y) \ +{ \ + return &image->data[x + y*image->stride / sizeof(T)]; \ +} \ + \ +static inline \ +const T* mrcal_image_ ## Tname ## _at_const(const mrcal_image_ ## Tname ## _t* image, int x, int y) \ +{ \ + return &image->data[x + y*image->stride / sizeof(T)]; \ +} \ + \ +static inline \ +mrcal_image_ ## Tname ## _t \ +mrcal_image_ ## Tname ## _crop(mrcal_image_ ## Tname ## _t* image, \ + int x0, int y0, \ + int w, int h) \ +{ \ + return (mrcal_image_ ## Tname ## _t){ .w = w, \ + .h = h, \ + .stride = image->stride, \ + .data = mrcal_image_ ## Tname ## _at(image,x0,y0) }; \ +} + +#define MRCAL_IMAGE_SAVE_LOAD_DECLARE(T, Tname) \ +bool mrcal_image_ ## Tname ## _save (const char* filename, const mrcal_image_ ## Tname ## _t* image); \ +bool mrcal_image_ ## Tname ## _load( mrcal_image_ ## Tname ## _t* image, const char* filename); + + +// Common images types +MRCAL_IMAGE_DECLARE(uint8_t, uint8); +MRCAL_IMAGE_DECLARE(uint16_t, uint16); +MRCAL_IMAGE_DECLARE(mrcal_bgr_t, bgr); +MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint8_t, uint8); +MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint16_t, uint16); +MRCAL_IMAGE_SAVE_LOAD_DECLARE(mrcal_bgr_t, bgr); + +// Uncommon types. Not everything supports these +MRCAL_IMAGE_DECLARE(int8_t, int8); +MRCAL_IMAGE_DECLARE(int16_t, int16); + +MRCAL_IMAGE_DECLARE(int32_t, int32); +MRCAL_IMAGE_DECLARE(uint32_t, uint32); +MRCAL_IMAGE_DECLARE(int64_t, int64); +MRCAL_IMAGE_DECLARE(uint64_t, uint64); + +MRCAL_IMAGE_DECLARE(float, float); +MRCAL_IMAGE_DECLARE(double, double); + +// Load the image into whatever type is stored on disk +bool mrcal_image_anytype_load(// output + // This is ONE of the known types + mrcal_image_uint8_t* image, + int* bits_per_pixel, + int* channels, + // input + const char* filename); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h new file mode 100644 index 0000000000..8fc2857758 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h @@ -0,0 +1,109 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +// THESE ARE NOT A PART OF THE EXTERNAL API. Exported for the mrcal python +// wrapper only + +// These models have no precomputed data +typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_CAHVORE__precomputed_t; + +// The splined stereographic models configuration parameters can be used to +// compute the segment size. I cache this computation +typedef struct +{ + // The distance between adjacent knots (1 segment) is u_per_segment = + // 1/segments_per_u + double segments_per_u; +} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t; + +typedef struct +{ + bool ready; + union + { +#define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed; + MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT) +#undef PRECOMPUTED_STRUCT + }; +} mrcal_projection_precomputed_t; + + +void _mrcal_project_internal_opencv( // outputs + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, // may be NULL + double* dq_dintrinsics_nocore, // may be NULL + + // inputs + const mrcal_point3_t* p, + int N, + const double* intrinsics, + int Nintrinsics); +bool _mrcal_project_internal( // out + mrcal_point2_t* q, + + // Stored as a row-first array of shape (N,2,3). Each + // trailing ,3 dimension element is a mrcal_point3_t + mrcal_point3_t* dq_dp, + // core, distortions concatenated. Stored as a row-first + // array of shape (N,2,Nintrinsics). This is a DENSE array. + // High-parameter-count lens models have very sparse + // gradients here, and the internal project() function + // returns those sparsely. For now THIS function densifies + // all of these + double* dq_dintrinsics, + + // in + const mrcal_point3_t* p, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + + int Nintrinsics, + const mrcal_projection_precomputed_t* precomputed); +void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed, + const mrcal_lensmodel_t* lensmodel); +bool _mrcal_unproject_internal( // out + mrcal_point3_t* out, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + const mrcal_projection_precomputed_t* precomputed); + +// Report the number of non-zero entries in the optimization jacobian +int _mrcal_num_j_nonzero(int Nobservations_board, + int Nobservations_point, + + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h new file mode 100644 index 0000000000..df987b6486 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h @@ -0,0 +1,396 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +#include +#include + +#include "basic-geometry.h" + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Lens models +//////////////////////////////////////////////////////////////////////////////// + +// These are an "X macro": https://en.wikipedia.org/wiki/X_Macro +// +// The supported lens models and their parameter counts. Models with a +// configuration may have a dynamic parameter count; this is indicated here with +// a <0 value. These models report their parameter counts in the +// LENSMODEL_XXX__lensmodel_num_params() function, called by +// mrcal_lensmodel_num_params(). +#define MRCAL_LENSMODEL_NOCONFIG_LIST(_) \ + _(LENSMODEL_PINHOLE, 4) \ + _(LENSMODEL_STEREOGRAPHIC, 4) /* Simple stereographic-only model */ \ + _(LENSMODEL_LONLAT, 4) \ + _(LENSMODEL_LATLON, 4) \ + _(LENSMODEL_OPENCV4, 8) \ + _(LENSMODEL_OPENCV5, 9) \ + _(LENSMODEL_OPENCV8, 12) \ + _(LENSMODEL_OPENCV12, 16) /* available in OpenCV >= 3.0.0) */ \ + _(LENSMODEL_CAHVOR, 9) +#define MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_) \ + _(LENSMODEL_CAHVORE, 12) +#define MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_) \ + _(LENSMODEL_SPLINED_STEREOGRAPHIC, -1) +#define MRCAL_LENSMODEL_LIST(_) \ + MRCAL_LENSMODEL_NOCONFIG_LIST(_) \ + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_) \ + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_) + + +// parametric models have no extra configuration +typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__config_t; + +#define _MRCAL_ITEM_DEFINE_ELEMENT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) type name bitfield; + +// Configuration for CAHVORE. These are given as an an +// "X macro": https://en.wikipedia.org/wiki/X_Macro +#define MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_, cookie) \ + _(linearity, double, "d", ".2f", "lf", , cookie) +typedef struct +{ + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, ) +} mrcal_LENSMODEL_CAHVORE__config_t; + +// Configuration for the splined stereographic models. These are given as an an +// "X macro": https://en.wikipedia.org/wiki/X_Macro +#define MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_, cookie) \ + /* Maximum degree of each 1D polynomial. This is almost certainly 2 */ \ + /* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */ \ + _(order, uint16_t, "H", PRIu16,SCNu16, , cookie) \ + /* We have a Nx by Ny grid of control points */ \ + _(Nx, uint16_t, "H", PRIu16,SCNu16, , cookie) \ + _(Ny, uint16_t, "H", PRIu16,SCNu16, , cookie) \ + /* The horizontal field of view. Not including fov_y. It's proportional with */ \ + /* Ny and Nx */ \ + _(fov_x_deg, uint16_t, "H", PRIu16,SCNu16, , cookie) +typedef struct +{ + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, ) +} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t; + + +// An X-macro-generated enum mrcal_lensmodel_type_t. This has an element for +// each entry in MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended). This lensmodel +// type selects the lens model, but does NOT provide the configuration. +// mrcal_lensmodel_t does that. +#define _LIST_WITH_COMMA(s,n) ,MRCAL_ ## s +typedef enum + { // Generic error. Rarely used in current mrcal + MRCAL_LENSMODEL_INVALID = -2, + + // Configuration parsing failed + MRCAL_LENSMODEL_INVALID_BADCONFIG = -1, + + // A configuration was required, but was missing entirely + MRCAL_LENSMODEL_INVALID_MISSINGCONFIG = -3, + + // The model type wasn't known + MRCAL_LENSMODEL_INVALID_TYPE = -4, + + // Dummy entry. Must be -1 so that successive values start at 0 + _MRCAL_LENSMODEL_DUMMY = -1 + + // The rest, starting with 0 + MRCAL_LENSMODEL_LIST( _LIST_WITH_COMMA ) } mrcal_lensmodel_type_t; +#undef _LIST_WITH_COMMA + + +// Defines a lens model: the type AND the configuration values +typedef struct +{ + // The type of lensmodel. This is an enum, selecting elements of + // MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended) + mrcal_lensmodel_type_t type; + + // A union of all the possible configuration structures. We pick the + // structure type based on the value of "type + union + { +#define CONFIG_STRUCT(s,n) mrcal_ ##s##__config_t s##__config; + MRCAL_LENSMODEL_LIST(CONFIG_STRUCT) +#undef CONFIG_STRUCT + }; +} mrcal_lensmodel_t; + + +typedef union +{ + struct + { + double x2, y2; + }; + double values[2]; +} mrcal_calobject_warp_t; + +#define MRCAL_NSTATE_CALOBJECT_WARP ((int)((sizeof(mrcal_calobject_warp_t)/sizeof(double)))) + +//// ADD CHANGES TO THE DOCS IN lensmodels.org +//// +// An X-macro-generated mrcal_lensmodel_metadata_t. Each lens model type has +// some metadata that describes its inherent properties. These properties can be +// queried by calling mrcal_lensmodel_metadata() in C and +// mrcal.lensmodel_metadata_and_config() in Python. The available properties and +// their meaning are listed in MRCAL_LENSMODEL_META_LIST() +#define MRCAL_LENSMODEL_META_LIST(_, cookie) \ + /* If true, this model contains an "intrinsics core". This is described */ \ + /* in mrcal_intrinsics_core_t. If present, the 4 core parameters ALWAYS */ \ + /* appear at the start of a model's parameter vector */ \ + _(has_core, bool, "i",,, :1, cookie) \ + \ + /* Whether a model is able to project points behind the camera */ \ + /* (z<0 in the camera coordinate system). Models based on a pinhole */ \ + /* projection (pinhole, OpenCV, CAHVOR(E)) cannot do this. models based */ \ + /* on a stereographic projection (stereographic, splined stereographic) */ \ + /* can */ \ + _(can_project_behind_camera, bool, "i",,, :1, cookie) \ + \ + /* Whether gradients are available for this model. Currently only */ \ + /* CAHVORE does not have gradients */ \ + _(has_gradients, bool, "i",,, :1, cookie) \ + \ + /* Whether this is a noncentral model.Currently the only noncentral */ \ + /* model we have is CAHVORE. There will be more later. */ \ + _(noncentral, bool, "i",,, :1, cookie) + +typedef struct +{ + MRCAL_LENSMODEL_META_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, ) +} mrcal_lensmodel_metadata_t; + + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Optimization +//////////////////////////////////////////////////////////////////////////////// + +// Used to specify which camera is making an observation. The "intrinsics" index +// is used to identify a specific camera, while the "extrinsics" index is used +// to locate a camera in space. If I have a camera that is moving over time, the +// intrinsics index will remain the same, while the extrinsics index will change +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: there should be a pool of these, and I should be indexing that pool" +#endif +typedef struct +{ + // indexes the intrinsics array + int intrinsics; + // indexes the extrinsics array. -1 means "at coordinate system reference" + int extrinsics; +} mrcal_camera_index_t; + + +// An observation of a calibration board. Each "observation" is ONE camera +// observing a board +typedef struct +{ + // which camera is making this observation + mrcal_camera_index_t icam; + + // indexes the "frames" array to select the pose of the calibration object + // being observed + int iframe; +} mrcal_observation_board_t; + +// An observation of a discrete point. Each "observation" is ONE camera +// observing a single point in space +typedef struct +{ + // which camera is making this observation + mrcal_camera_index_t icam; + + // indexes the "points" array to select the position of the point being + // observed + int i_point; +} mrcal_observation_point_t; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: triangulated points into a pool. maybe allowing the intrinsics to move in the process" +#endif + +// An observation of a discrete point where the point itself is NOT a part of +// the optimization, but computed implicitly via triangulation. This structure +// is very similar to mrcal_observation_point_t, except instead of i_point +// identifying the point being observed we have +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: FINISH DOC" +#endif +typedef struct +{ + // which camera is making this observation + mrcal_camera_index_t icam; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: DOCUMENT. CAN THIS BIT FIELD BE PACKED NICELY?" +#endif + // Set if this is the last camera observing this point. Any set of N>=2 + // cameras can observe any point. All observations of a given point are + // stored consecutively, the last one being noted by this bit +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: do I really need this? I cannot look at the next observation to determine when this one is done?" +#endif + bool last_in_set : 1; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: this is temporary. Should be a weight in observations_point_pool like all the other observations" +#endif + bool outlier : 1; + + // Observed pixel coordinates. This works just like elements of + // observations_board_pool and observations_point_pool + mrcal_point3_t px; +} mrcal_observation_point_triangulated_t; + + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: need a function to identify a vanilla calibration problem. It needs to not include any triangulated points. The noise propagation is different" +#endif + + +// Bits indicating which parts of the optimization problem being solved. We can +// ask mrcal to solve for ALL the lens parameters and ALL the geometry and +// everything else. OR we can ask mrcal to lock down some part of the +// optimization problem, and to solve for the rest. If any variables are locked +// down, we use their initial values passed-in to mrcal_optimize() +typedef struct +{ + // If true, we solve for the intrinsics core. Applies only to those models + // that HAVE a core (fx,fy,cx,cy) + bool do_optimize_intrinsics_core : 1; + + // If true, solve for the non-core lens parameters + bool do_optimize_intrinsics_distortions : 1; + + // If true, solve for the geometry of the cameras + bool do_optimize_extrinsics : 1; + + // If true, solve for the poses of the calibration object + bool do_optimize_frames : 1; + + // If true, optimize the shape of the calibration object + bool do_optimize_calobject_warp : 1; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: Need finer-grained regularization flags" +// #warning "triangulated-solve: Regularization flags should reflect do_optimize stuff and Ncameras stuff" +#endif + // If true, apply the regularization terms in the solver + bool do_apply_regularization : 1; + + // Whether to try to find NEW outliers. The outliers given on + // input are respected regardless + bool do_apply_outlier_rejection : 1; + + // Pull the distance between the first two cameras to 1.0 + bool do_apply_regularization_unity_cam01: 1; + +} mrcal_problem_selections_t; + +// Constants used in a mrcal optimization. This is similar to +// mrcal_problem_selections_t, but contains numerical values rather than just +// bits +typedef struct +{ + // The minimum distance of an observed discrete point from its observing + // camera. Any observation of a point below this range will be penalized to + // encourage the optimizer to move the point further away from the camera + double point_min_range; + + + // The maximum distance of an observed discrete point from its observing + // camera. Any observation of a point abive this range will be penalized to + // encourage the optimizer to move the point closer to the camera + double point_max_range; +} mrcal_problem_constants_t; + + +// An X-macro-generated mrcal_stats_t. This structure is returned by the +// optimizer, and contains some statistics about the optimization +#define MRCAL_STATS_ITEM(_) \ + /* The RMS error of the optimized fit at the optimum. Generally the residual */ \ + /* vector x contains error values for each element of q, so N observed pixels */ \ + /* produce 2N measurements: len(x) = 2*N. And the RMS error is */ \ + /* sqrt( norm2(x) / N ) */ \ + _(double, rms_reproj_error__pixels, PyFloat_FromDouble) \ + \ + /* How many pixel observations were thrown out as outliers. Each pixel */ \ + /* observation produces two measurements. Note that this INCLUDES any */ \ + /* outliers that were passed-in at the start */ \ + _(int, Noutliers_board, PyLong_FromLong) \ + \ + /* How many pixel observations were thrown out as outliers. Each pixel */ \ + /* observation produces two measurements. Note that this INCLUDES any */ \ + /* outliers that were passed-in at the start */ \ + _(int, Noutliers_triangulated_point, PyLong_FromLong) +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: implement stats.Noutliers_triangulated_point; add to c-api.org" +#endif +#define MRCAL_STATS_ITEM_DEFINE(type, name, pyconverter) type name; +typedef struct +{ + MRCAL_STATS_ITEM(MRCAL_STATS_ITEM_DEFINE) +} mrcal_stats_t; + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Layout of the measurement and state vectors +//////////////////////////////////////////////////////////////////////////////// + +// The "intrinsics core" of a camera. This defines the final step of a +// projection operation. For instance with a pinhole model we have +// +// q[0] = focal_xy[0] * x/z + center_xy[0] +// q[1] = focal_xy[1] * y/z + center_xy[1] +typedef struct +{ + double focal_xy [2]; + double center_xy[2]; +} mrcal_intrinsics_core_t; + + +// structure containing a camera pose + lens model. Used for .cameramodel +// input/output +#define MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS \ + double rt_cam_ref[6]; \ + unsigned int imagersize[2]; \ + mrcal_lensmodel_t lensmodel \ + +typedef struct +{ + MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS; + // mrcal_cameramodel_t works for all lensmodels, so its intrinsics count is + // not known at compile time. mrcal_cameramodel_t is thus usable only as + // part of a larger structure or as a mrcal_cameramodel_t* argument to + // functions. To allocate new mrcal_cameramodel_t objects, use + // mrcal_cameramodel_LENSMODEL_XXX_t or malloc() with the proper intrinsics + // size taken into account + double intrinsics[0]; +} mrcal_cameramodel_t; + + +#define DEFINE_mrcal_cameramodel_MODEL_t(s,n) \ +typedef union \ +{ \ + mrcal_cameramodel_t m; \ + struct \ + { \ + MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS; \ + double intrinsics[n]; \ + }; \ +} mrcal_cameramodel_ ## s ## _t; + + +MRCAL_LENSMODEL_NOCONFIG_LIST( DEFINE_mrcal_cameramodel_MODEL_t) +MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(DEFINE_mrcal_cameramodel_MODEL_t) diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h new file mode 100644 index 0000000000..b61b487835 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h @@ -0,0 +1,924 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +#include +#include + +#include "mrcal-types.h" +#include "poseutils.h" +#include "stereo.h" +#include "triangulation.h" +#include "mrcal-image.h" + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Lens models +//////////////////////////////////////////////////////////////////////////////// +#ifdef __cplusplus +extern "C" { +#endif +// Return an array of strings listing all the available lens models +// +// These are all "unconfigured" strings that use "..." placeholders for any +// configuration values. Each returned string is a \0-terminated const char*. The +// end of the list is signified by a NULL pointer +const char* const* mrcal_supported_lensmodel_names( void ); // NULL-terminated array of char* strings + + +// Return true if the given mrcal_lensmodel_type_t specifies a valid lens model + +static bool mrcal_lensmodel_type_is_valid(mrcal_lensmodel_type_t t) +{ + return t >= 0; +} + +// Evaluates to true if the given lens model is one of the supported OpenCV +// types +#define MRCAL_LENSMODEL_IS_OPENCV(d) (MRCAL_LENSMODEL_OPENCV4 <= (d) && (d) <= MRCAL_LENSMODEL_OPENCV12) + + +// Return a string describing a lens model. +// +// This function returns a static string. For models with no configuration, this +// is the FULL string for that model. For models with a configuration, the +// configuration values have "..." placeholders. These placeholders mean that +// the resulting strings do not define a lens model fully, and cannot be +// converted to a mrcal_lensmodel_t with mrcal_lensmodel_from_name() +// +// This is the inverse of mrcal_lensmodel_type_from_name() +const char* mrcal_lensmodel_name_unconfigured( const mrcal_lensmodel_t* lensmodel ); + + +// Return a CONFIGURED string describing a lens model. +// +// This function generates a fully-configured string describing the given lens +// model. For models with no configuration, this is just the static string +// returned by mrcal_lensmodel_name_unconfigured(). For models that have a +// configuration, however, the configuration values are filled-in. The resulting +// string may be converted back into a mrcal_lensmodel_t by calling +// mrcal_lensmodel_from_name(). +// +// This function writes the string into the given buffer "out". The size of the +// buffer is passed in the "size" argument. The meaning of "size" is as with +// snprintf(), which is used internally. Returns true on success +// +// This is the inverse of mrcal_lensmodel_from_name() +bool mrcal_lensmodel_name( char* out, int size, + const mrcal_lensmodel_t* lensmodel ); + + +// Parse the lens model type from a lens model name string +// +// The configuration is ignored. Thus this function works even if the +// configuration is missing or unparseable. Unknown model names return +// MRCAL_LENSMODEL_INVALID_TYPE +// +// This is the inverse of mrcal_lensmodel_name_unconfigured() +mrcal_lensmodel_type_t mrcal_lensmodel_type_from_name( const char* name ); + + +// Parse the full configured lens model from a lens model name string +// +// The lens mode type AND the configuration are read into a mrcal_lensmodel_t +// structure, which this function returns. +// +// On error returns false with lensmodel->type set to MRCAL_LENSMODEL_INVALID_... +// +// This is the inverse of mrcal_lensmodel_name() +bool mrcal_lensmodel_from_name( // output + mrcal_lensmodel_t* lensmodel, + + // input + const char* name ); + +// Return a structure containing a model's metadata +// +// The available metadata is described in the definition of the +// MRCAL_LENSMODEL_META_LIST() macro +mrcal_lensmodel_metadata_t mrcal_lensmodel_metadata( const mrcal_lensmodel_t* lensmodel ); + + +// Return the number of parameters required to specify a given lens model +// +// For models that have a configuration, the parameter count value generally +// depends on the configuration. For instance, splined models use the model +// parameters as the spline control points, so the spline density (specified in +// the configuration) directly affects how many parameters such a model requires +int mrcal_lensmodel_num_params( const mrcal_lensmodel_t* lensmodel ); + + +// Return the locations of x and y spline knots + +// Splined models are defined by the locations of their control points. These +// are arranged in a grid, the size and density of which is set by the model +// configuration. We fill-in the x knot locations into ux[] and the y locations +// into uy[]. ux[] and uy[] must be large-enough to hold configuration->Nx and +// configuration->Ny values respectively. +// +// This function applies to splined models only. Returns true on success +bool mrcal_knots_for_splined_models( double* ux, double* uy, + const mrcal_lensmodel_t* lensmodel); + + + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Projections +//////////////////////////////////////////////////////////////////////////////// + +// Project the given camera-coordinate-system points +// +// Compute a "projection", a mapping of points defined in the camera coordinate +// system to their observed pixel coordinates. If requested, gradients are +// computed as well. +// +// We project N 3D points p to N 2D pixel coordinates q using the given +// lensmodel and intrinsics parameter values. +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +// +// if (dq_dintrinsics != NULL) we report the gradient dq/dintrinsics in a dense +// (N,2,Nintrinsics) array. Note that splined models have very high Nintrinsics +// and very sparse gradients. THIS function reports the gradients densely, +// however, so it is inefficient for splined models. +// +// This function supports CAHVORE distortions only if we don't ask for any +// gradients +// +// Projecting out-of-bounds points (beyond the field of view) returns undefined +// values. Generally things remain continuous even as we move off the imager +// domain. Pinhole-like projections will work normally if projecting a point +// behind the camera. Splined projections clamp to the nearest spline segment: +// the projection will fly off to infinity quickly since we're extrapolating a +// polynomial, but the function will remain continuous. +bool mrcal_project( // out + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, + double* dq_dintrinsics, + + // in + const mrcal_point3_t* p, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics); + + +// Unproject the given pixel coordinates +// +// Compute an "unprojection", a mapping of pixel coordinates to the camera +// coordinate system. +// +// We unproject N 2D pixel coordinates q to N 3D direction vectors v using the +// given lensmodel and intrinsics parameter values. The returned vectors v are +// not normalized, and may have any length. + +// This is the "reverse" direction, so an iterative nonlinear optimization is +// performed internally to compute this result. This is much slower than +// mrcal_project(). For OpenCV models specifically, OpenCV has +// cvUndistortPoints() (and cv2.undistortPoints()), but these are unreliable: +// https://github.com/opencv/opencv/issues/8811 +// +// This function does NOT support CAHVORE +bool mrcal_unproject( // out + mrcal_point3_t* v, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics); + + +// Project the given camera-coordinate-system points using a pinhole +// model. See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_pinhole( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, + + // input + const mrcal_point3_t* p, + int N, + const double* fxycxy); + + +// Unproject the given pixel coordinates using a pinhole model. +// See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole +// +// This is a simplified special case of mrcal_unproject(). We unproject N 2D +// pixel coordinates q to N camera-coordinate-system vectors v. The returned +// vectors v are not normalized, and may have any length. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_pinhole( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + +// Project the given camera-coordinate-system points using a stereographic +// model. See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_stereographic( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, + + // input + const mrcal_point3_t* p, + int N, + const double* fxycxy); + + +// Unproject the given pixel coordinates using a stereographic model. +// See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic +// +// This is a simplified special case of mrcal_unproject(). We unproject N 2D +// pixel coordinates q to N camera-coordinate-system vectors v. The returned +// vectors v are not normalized, and may have any length. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_stereographic( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + + +// Project the given camera-coordinate-system points using an equirectangular +// projection. See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_lonlat( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dv, // May be NULL. Each point + // gets a block of 2 mrcal_point3_t + // objects + + // input + const mrcal_point3_t* v, + int N, + const double* fxycxy); + +// Unproject the given pixel coordinates using an equirectangular projection. +// See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat +// +// This is a simplified special case of mrcal_unproject(). We unproject N 2D +// pixel coordinates q to N camera-coordinate-system vectors v. The returned +// vectors v are normalized. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_lonlat( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, // May be NULL. Each point + // gets a block of 3 mrcal_point2_t + // objects + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + + +// Project the given camera-coordinate-system points using a transverse +// equirectangular projection. See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_latlon( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dv, // May be NULL. Each point + // gets a block of 2 mrcal_point3_t + // objects + + // input + const mrcal_point3_t* v, + int N, + const double* fxycxy); + +// Unproject the given pixel coordinates using a transverse equirectangular +// projection. See the docs for projection details: +// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon +// +// This is a simplified special case of mrcal_unproject(). We unproject N 2D +// pixel coordinates q to N camera-coordinate-system vectors v. The returned +// vectors v are normalized. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_latlon( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, // May be NULL. Each point + // gets a block of 3 mrcal_point2_t + // objects + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + + + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Optimization +//////////////////////////////////////////////////////////////////////////////// + +// Return the number of parameters needed in optimizing the given lens model +// +// This is identical to mrcal_lensmodel_num_params(), but takes into account the +// problem selections. Any intrinsics parameters locked down in the +// mrcal_problem_selections_t do NOT count towards the optimization parameters +int mrcal_num_intrinsics_optimization_params( mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel ); + + +// Scales a state vector to the packed, unitless form used by the optimizer +// +// In order to make the optimization well-behaved, we scale all the variables in +// the state and the gradients before passing them to the optimizer. The internal +// optimization library thus works only with unitless (or "packed") data. +// +// This function takes an (Nstate,) array of full-units values b[], and scales +// it to produce packed data. This function applies the scaling directly to the +// input array; the input is modified, and nothing is returned. +// +// This is the inverse of mrcal_unpack_solver_state_vector() +void mrcal_pack_solver_state_vector( // out, in + double* b, + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); + + +// Scales a state vector from the packed, unitless form used by the optimizer +// +// In order to make the optimization well-behaved, we scale all the variables in +// the state and the gradients before passing them to the optimizer. The internal +// optimization library thus works only with unitless (or "packed") data. +// +// This function takes an (Nstate,) array of unitless values b[], and scales it +// to produce full-units data. This function applies the scaling directly to the +// input array; the input is modified, and nothing is returned. +// +// This is the inverse of mrcal_pack_solver_state_vector() +void mrcal_unpack_solver_state_vector( // out, in + double* b, // unitless state on input, + // scaled, meaningful state on + // output + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); + + +// Reports the icam_extrinsics corresponding to a given icam_intrinsics. +// +// If we're solving a vanilla calibration problem (stationary cameras observing +// a moving calibration object), each camera has a unique intrinsics index and a +// unique extrinsics index. This function reports the latter, given the former. +// +// On success, the result is written to *icam_extrinsics, and we return true. If +// the given camera is at the reference coordinate system, it has no extrinsics, +// and we report -1. +// +// If we have moving cameras (NOT a vanilla calibration problem), there isn't a +// single icam_extrinsics for a given icam_intrinsics, and we report an error by +// returning false +bool mrcal_corresponding_icam_extrinsics(// out + int* icam_extrinsics, + + // in + int icam_intrinsics, + int Ncameras_intrinsics, + int Ncameras_extrinsics, + int Nobservations_board, + const mrcal_observation_board_t* observations_board, + int Nobservations_point, + const mrcal_observation_point_t* observations_point); + +// Solve the given optimization problem +// +// This is the entry point to the mrcal optimization routine. The argument list +// is commented. +mrcal_stats_t +mrcal_optimize( // out + // Each one of these output pointers may be NULL + // Shape (Nstate,) + double* b_packed, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_b_packed, + + // Shape (Nmeasurements,) + double* x, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x, + + // out, in + + // These are a seed on input, solution on output + + // intrinsics is a concatenation of the intrinsics core and the + // distortion params. The specific distortion parameters may + // vary, depending on lensmodel, so this is a variable-length + // structure + double* intrinsics, // Ncameras_intrinsics * NlensParams + mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + mrcal_point3_t* points, // Npoints of these. In the reference frame + mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + int Nobservations_board, + int Nobservations_point, + + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + // All the board pixel observations, in an array of shape + // + // ( Nobservations_board, + // calibration_object_height_n, + // calibration_object_width_n ) + // + // .x, .y are the pixel observations .z is the weight of the + // observation. Most of the weights are expected to be 1.0. Less + // precise observations have lower weights. + // + // .z<0 indicates that this is an outlier. This is respected on + // input (even if !do_apply_outlier_rejection). New outliers are + // marked with .z<0 on output, so this isn't const + mrcal_point3_t* observations_board_pool, + + // Same this, but for discrete points. Array of shape + // + // ( Nobservations_point,) + mrcal_point3_t* observations_point_pool, + + const mrcal_lensmodel_t* lensmodel, + const int* imagersizes, // Ncameras_intrinsics*2 of these + mrcal_problem_selections_t problem_selections, + const mrcal_problem_constants_t* problem_constants, + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n, + bool verbose, + + bool check_gradient); + + +// These are cholmod_sparse, cholmod_factor, cholmod_common. I don't want to +// include the full header that defines these in mrcal.h, and I don't need to: +// mrcal.h just needs to know that these are a structure +struct cholmod_sparse_struct; +struct cholmod_factor_struct; +struct cholmod_common_struct; + +// Evaluate the value of the callback function at the given operating point +// +// The main optimization routine in mrcal_optimize() searches for optimal +// parameters by repeatedly calling a function to evaluate each hypothethical +// parameter set. This evaluation function is available by itself here, +// separated from the optimization loop. The arguments are largely the same as +// those to mrcal_optimize(), but the inputs are all read-only It is expected +// that this will be called from Python only. +bool mrcal_optimizer_callback(// out + + // These output pointers may NOT be NULL, unlike + // their analogues in mrcal_optimize() + + // Shape (Nstate,) + double* b_packed, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_b_packed, + + // Shape (Nmeasurements,) + double* x, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x, + + // output Jacobian. May be NULL if we don't need + // it. This is the unitless Jacobian, used by the + // internal optimization routines + struct cholmod_sparse_struct* Jt, + + + // in + + // intrinsics is a concatenation of the intrinsics core + // and the distortion params. The specific distortion + // parameters may vary, depending on lensmodel, so + // this is a variable-length structure + const double* intrinsics, // Ncameras_intrinsics * NlensParams + const mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + const mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + const mrcal_point3_t* points, // Npoints of these. In the reference frame + const mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + int Nobservations_board, + int Nobservations_point, + + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + // All the board pixel observations, in an array of shape + // + // ( Nobservations_board, + // calibration_object_height_n, + // calibration_object_width_n ) + // + // .x, .y are the pixel observations .z is the + // weight of the observation. Most of the weights + // are expected to be 1.0. Less precise + // observations have lower weights. + // + // .z<0 indicates that this is an outlier + const mrcal_point3_t* observations_board_pool, + + // Same this, but for discrete points. Array of shape + // + // ( Nobservations_point,) + const mrcal_point3_t* observations_point_pool, + + const mrcal_lensmodel_t* lensmodel, + const int* imagersizes, // Ncameras_intrinsics*2 of these + mrcal_problem_selections_t problem_selections, + const mrcal_problem_constants_t* problem_constants, + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n, + bool verbose); + +bool mrcal_drt_ref_refperturbed__dbpacked(// output + // Shape (6,Nstate_frames) + double* Kpackedf, + int Kpackedf_stride0, // in bytes. <= 0 means "contiguous" + int Kpackedf_stride1, // in bytes. <= 0 means "contiguous" + + // Shape (6,Nstate_points) + double* Kpackedp, + int Kpackedp_stride0, // in bytes. <= 0 means "contiguous" + int Kpackedp_stride1, // in bytes. <= 0 means "contiguous" + + // Shape (6,Nstate_calobject_warp) + double* Kpackedcw, + int Kpackedcw_stride0, // in bytes. <= 0 means "contiguous" + int Kpackedcw_stride1, // in bytes. <= 0 means "contiguous" + + // inputs + // stuff that describes this solve + const double* b_packed, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_b_packed, + + // The unitless (packed) Jacobian, + // used by the internal optimization + // routines cholmod_analyze() and + // cholmod_factorize() require + // non-const + /* const */ + struct cholmod_sparse_struct* Jt, + + // meta-parameters + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + int Nobservations_board, + int Nobservations_point, + + const mrcal_lensmodel_t* lensmodel, + mrcal_problem_selections_t problem_selections, + + int calibration_object_width_n, + int calibration_object_height_n); + + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Layout of the measurement and state vectors +//////////////////////////////////////////////////////////////////////////////// + +// The optimization routine tries to minimize the length of the measurement +// vector x by moving around the state vector b. +// +// Depending on the specific optimization problem being solved and the +// mrcal_problem_selections_t, the state vector may contain any of +// - The lens parameters +// - The geometry of the cameras +// - The geometry of the observed chessboards and discrete points +// - The chessboard shape +// +// The measurement vector may contain +// - The errors in observations of the chessboards +// - The errors in observations of discrete points +// - The penalties in the solved point positions +// - The regularization terms +// +// Given the problem selections and a vector b or x it is often useful to know +// where specific quantities lie in those vectors. We have 4 sets of functions +// to answer such questions: +// +// int mrcal_measurement_index_THING() +// Returns the index in the measurement vector x where the contiguous block of +// values describing the THING begins. THING is any of +// - boards +// - points +// - regularization +// +// int mrcal_num_measurements_THING() +// Returns the number of values in the contiguous block in the measurement +// vector x that describe the given THING. THING is any of +// - boards +// - points +// - regularization +// +// int mrcal_state_index_THING() +// Returns the index in the state vector b where the contiguous block of +// values describing the THING begins. THING is any of +// - intrinsics +// - extrinsics +// - frames +// - points +// - calobject_warp +// If we're not optimizing the THING, return <0 +// +// int mrcal_num_states_THING() +// Returns the number of values in the contiguous block in the state +// vector b that describe the given THING. THING is any of +// - intrinsics +// - extrinsics +// - frames +// - points +// - calobject_warp +// If we're not optimizing the THING, return 0 +int mrcal_measurement_index_boards(int i_observation_board, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n); +int mrcal_num_measurements_boards(int Nobservations_board, + int calibration_object_width_n, + int calibration_object_height_n); +int mrcal_measurement_index_points(int i_observation_point, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n); +int mrcal_num_measurements_points(int Nobservations_point); +int mrcal_measurement_index_points_triangulated(int i_point_triangulated, + int Nobservations_board, + int Nobservations_point, + + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n); +int mrcal_num_measurements_points_triangulated(// May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated); +int mrcal_num_measurements_points_triangulated_initial_Npoints(// May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + // Only consider the leading Npoints. If Npoints < 0: take ALL the points + int Npoints); +bool mrcal_decode_observation_indices_points_triangulated( + // output + int* iobservation0, + int* iobservation1, + int* iobservation_point0, + int* Nobservations_this_point, + int* Nmeasurements_this_point, + int* ipoint, + + // input + const int imeasurement, + + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated); + +int mrcal_measurement_index_regularization(// May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, int Nobservations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); + +int mrcal_num_measurements(int Nobservations_board, + int Nobservations_point, + + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); + +int mrcal_num_states(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_state_index_intrinsics(int icam_intrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_intrinsics(int Ncameras_intrinsics, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_state_index_extrinsics(int icam_extrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_extrinsics(int Ncameras_extrinsics, + mrcal_problem_selections_t problem_selections); +int mrcal_state_index_frames(int iframe, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_frames(int Nframes, + mrcal_problem_selections_t problem_selections); +int mrcal_state_index_points(int i_point, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_points(int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections); +int mrcal_state_index_calobject_warp(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_calobject_warp(mrcal_problem_selections_t problem_selections, + int Nobservations_board); + + +// if len>0, the string doesn't need to be 0-terminated. If len<=0, the end of +// the buffer IS indicated by a '\0' byte +// +// return NULL on error +mrcal_cameramodel_t* mrcal_read_cameramodel_string(const char* string, int len); +mrcal_cameramodel_t* mrcal_read_cameramodel_file (const char* filename); +void mrcal_free_cameramodel(mrcal_cameramodel_t** cameramodel); + +bool mrcal_write_cameramodel_file(const char* filename, + const mrcal_cameramodel_t* cameramodel); + +#ifdef __cplusplus +} // extern "C" +#endif +#define DECLARE_mrcal_apply_color_map(T,Tname) \ + bool mrcal_apply_color_map_##Tname( \ + mrcal_image_bgr_t* out, \ + const mrcal_image_##Tname##_t* in, \ + \ + /* If true, I set in_min/in_max from the */ \ + /* min/max of the input data */ \ + const bool auto_min, \ + const bool auto_max, \ + \ + /* If true, I implement gnuplot's default 7,5,15 mapping. */ \ + /* This is a reasonable default choice. */ \ + /* function_red/green/blue are ignored if true */ \ + const bool auto_function, \ + \ + /* min/max input values to use if not */ \ + /* auto_min/auto_max */ \ + T in_min, /* will map to 0 */ \ + T in_max, /* will map to 255 */ \ + \ + /* The color mappings to use. If !auto_function */ \ + int function_red, \ + int function_green, \ + int function_blue) + +DECLARE_mrcal_apply_color_map(uint8_t, uint8); +DECLARE_mrcal_apply_color_map(uint16_t, uint16); +DECLARE_mrcal_apply_color_map(uint32_t, uint32); +DECLARE_mrcal_apply_color_map(uint64_t, uint64); + +DECLARE_mrcal_apply_color_map(int8_t, int8); +DECLARE_mrcal_apply_color_map(int16_t, int16); +DECLARE_mrcal_apply_color_map(int32_t, int32); +DECLARE_mrcal_apply_color_map(int64_t, int64); + +DECLARE_mrcal_apply_color_map(float, float); +DECLARE_mrcal_apply_color_map(double, double); + +#undef DECLARE_mrcal_apply_color_map + + + +// returns false on error +typedef bool (*mrcal_callback_sensor_link_t)(const uint16_t idx_to, + const uint16_t idx_from, + void* cookie); + +// Traverses a connectivity graph of sensors to find the best connection from +// the root sensor (idx==0) to every other sensor. This is useful to seed a +// problem with sparse connections, where every sensor doesn't have overlapping +// observations with every other sensor. See the docstring for +// mrcal.traverse_sensor_links() for details (that Python function wraps +// this one). Note: this C function takes a packed connectivity matrix (just the +// upper triangle stored), while the Python function takes a full (N,N) array, +// while assuming it is symmetric and has a 0 diagonal +// +// returns false on error +bool mrcal_traverse_sensor_links( const uint16_t Nsensors, + + // (N,N) symmetric matrix with a 0 diagonal. + // I store the upper triangle only, + // row-first: a 1D array of (N*(N-1)/2) + // values. use pairwise_index() to index + const uint16_t* connectivity_matrix, + const mrcal_callback_sensor_link_t cb, + void* cookie); + + +// Public ABI stuff, that's not for end-user consumption +#include "mrcal-internal.h" + diff --git a/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h b/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h new file mode 100644 index 0000000000..23b1c772cf --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h @@ -0,0 +1,586 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +#include + +// Unless specified all arrays stored in contiguous matrices in row-major order. +// +// All functions are defined using the mrcal_..._full() form, which supports +// non-contiguous input and output arrays, and some optional arguments. Strides +// are used to specify the array layout. +// +// All functions have a convenience wrapper macro that is a simpler way to call +// the function, usable with contiguous arrays and defaults. +// +// All the functions use double-precision floating point to store the data, and +// C ints to store strides. The strides are given in bytes. In the +// mrcal_..._full() functions, each array is followed by the strides, one per +// dimension. +// +// I have two different representations of pose transformations: +// +// - Rt is a concatenated (4,3) array: Rt = nps.glue(R,t, axis=-2). The +// transformation is R*x+t +// +// - rt is a concatenated (6,) array: rt = nps.glue(r,t, axis=-1). The +// transformation is R*x+t where R = R_from_r(r) +// +// I treat all vectors as column vectors, so matrix multiplication works from +// the left: to rotate a vector x by a rotation matrix R I have +// +// x_rotated = R * x + + +// Store an identity rotation matrix into the given (3,3) array +// +// This is simply an identity matrix +#define mrcal_identity_R(R) mrcal_identity_R_full(R,0,0) +void mrcal_identity_R_full(double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Store an identity rodrigues rotation into the given (3,) array +// +// This is simply an array of zeros +#define mrcal_identity_r(r) mrcal_identity_r_full(r,0) +void mrcal_identity_r_full(double* r, // (3,) array + int r_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Store an identity Rt transformation into the given (4,3) array +#define mrcal_identity_Rt(Rt) mrcal_identity_Rt_full(Rt,0,0) +void mrcal_identity_Rt_full(double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Store an identity rt transformation into the given (6,) array +#define mrcal_identity_rt(rt) mrcal_identity_rt_full(rt,0) +void mrcal_identity_rt_full(double* rt, // (6,) array + int rt_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Rotate the point x_in in a (3,) array by the rotation matrix R in a (3,3) +// array. This is simply the matrix-vector multiplication R x_in +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/dR is returned in a (3, 3,3) array J_R. Set to NULL if +// this is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply +// the matrix R. Set to NULL if this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_rotate_point_R( x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, false) +#define mrcal_rotate_point_R_inverted(x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, true) +void mrcal_rotate_point_R_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_R, // (3,3,3) array. May be NULL + int J_R_stride0, // in bytes. <= 0 means "contiguous" + int J_R_stride1, // in bytes. <= 0 means "contiguous" + int J_R_stride2, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* R, // (3,3) array. May be NULL + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_R corresponds + // to the input R + ); + +// Rotate the point x_in in a (3,) array by the rodrigues rotation in a (3,) +// array. +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/dr is returned in a (3,3) array J_r. Set to NULL if this +// is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. Set to NULL if +// this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_rotate_point_r( x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, false) +#define mrcal_rotate_point_r_inverted(x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, true) +void mrcal_rotate_point_r_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_r, // (3,3) array. May be NULL + int J_r_stride0, // in bytes. <= 0 means "contiguous" + int J_r_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r, // (3,) array. May be NULL + int r_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_r corresponds + // to the input r + ); + + +// Transform the point x_in in a (3,) array by the Rt transformation in a (4,3) +// array. +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/dRt is returned in a (3, 4,3) array J_Rt. Set to NULL if +// this is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply +// the matrix R. Set to NULL if this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_transform_point_Rt( x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, false) +#define mrcal_transform_point_Rt_inverted(x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, true) +void mrcal_transform_point_Rt_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_Rt, // (3,4,3) array. May be NULL + int J_Rt_stride0, // in bytes. <= 0 means "contiguous" + int J_Rt_stride1, // in bytes. <= 0 means "contiguous" + int J_Rt_stride2, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt, // (4,3) array. May be NULL + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // transformation in the opposite + // direction. J_Rt corresponds + // to the input Rt + ); + +// Transform the point x_in in a (3,) array by the rt transformation in a (6,) +// array. +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/drt is returned in a (3,6) array J_rt. Set to NULL if +// this is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply +// the matrix R. Set to NULL if this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_transform_point_rt( x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, false) +#define mrcal_transform_point_rt_inverted(x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, true) +void mrcal_transform_point_rt_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_rt, // (3,6) array. May be NULL + int J_rt_stride0, // in bytes. <= 0 means "contiguous" + int J_rt_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt, // (6,) array. May be NULL + int rt_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply the + // transformation in the + // opposite direction. + // J_rt corresponds to + // the input rt + ); + +// Convert a rotation matrix in a (3,3) array to a rodrigues vector in a (3,) +// array +// +// The result is returned in a (3,) array r +// +// The gradient dr/dR is returned in a (3, 3,3) array J. Set to NULL if this is +// not wanted +#define mrcal_r_from_R(r,J,R) mrcal_r_from_R_full(r,0,J,0,0,0,R,0,0) +void mrcal_r_from_R_full( // output + double* r, // (3,) vector + int r_stride0, // in bytes. <= 0 means "contiguous" + double* J, // (3,3,3) array. Gradient. May be NULL + int J_stride0, // in bytes. <= 0 means "contiguous" + int J_stride1, // in bytes. <= 0 means "contiguous" + int J_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Convert a rodrigues vector in a (3,) array to a rotation matrix in a (3,3) +// array +// +// The result is returned in a (3,3) array R +// +// The gradient dR/dr is returned in a (3,3 ,3) array J. Set to NULL if this is +// not wanted +#define mrcal_R_from_r(R,J,r) mrcal_R_from_r_full(R,0,0,J,0,0,0,r,0) +void mrcal_R_from_r_full( // outputs + double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // in bytes. <= 0 means "contiguous" + double* J, // (3,3,3) array. Gradient. May be NULL + int J_stride0, // in bytes. <= 0 means "contiguous" + int J_stride1, // in bytes. <= 0 means "contiguous" + int J_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* r, // (3,) vector + int r_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Invert a rotation matrix. This is a transpose +// +// The input is given in R_in in a (3,3) array +// +// The result is returned in a (3,3) array R_out +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_invert_R(R_out,R_in) mrcal_invert_R_full(R_out,0,0,R_in,0,0) +void mrcal_invert_R_full( // output + double* R_out, // (3,3) array + int R_out_stride0, // in bytes. <= 0 means "contiguous" + int R_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* R_in, // (3,3) array + int R_in_stride0, // in bytes. <= 0 means "contiguous" + int R_in_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Convert an Rt transformation in a (4,3) array to an rt transformation in a +// (6,) array +// +// The result is returned in a (6,) array rt +// +// The gradient dr/dR is returned in a (3, 3,3) array J_R. Set to NULL if this +// is not wanted +// +// The t terms are identical, so dt/dt = identity and I do not return it +// +// The r and R terms are independent of the t terms, so dr/dt and dt/dR are both +// 0, and I do not return them +#define mrcal_rt_from_Rt(rt,J_R,Rt) mrcal_rt_from_Rt_full(rt,0,J_R,0,0,0,Rt,0,0) +void mrcal_rt_from_Rt_full( // output + double* rt, // (6,) vector + int rt_stride0, // in bytes. <= 0 means "contiguous" + double* J_R, // (3,3,3) array. Gradient. May be NULL + // No J_t. It's always the identity + int J_R_stride0, // in bytes. <= 0 means "contiguous" + int J_R_stride1, // in bytes. <= 0 means "contiguous" + int J_R_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Convert an rt transformation in a (6,) array to an Rt transformation in a +// (4,3) array +// +// The result is returned in a (4,3) array Rt +// +// The gradient dR/dr is returned in a (3,3 ,3) array J_r. Set to NULL if this +// is not wanted +// +// The t terms are identical, so dt/dt = identity and I do not return it +// +// The r and R terms are independent of the t terms, so dR/dt and dt/dr are both +// 0, and I do not return them +#define mrcal_Rt_from_rt(Rt,J_r,rt) mrcal_Rt_from_rt_full(Rt,0,0,J_r,0,0,0,rt,0) +void mrcal_Rt_from_rt_full( // output + double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1, // in bytes. <= 0 means "contiguous" + double* J_r, // (3,3,3) array. Gradient. May be NULL + // No J_t. It's just the identity + int J_r_stride0, // in bytes. <= 0 means "contiguous" + int J_r_stride1, // in bytes. <= 0 means "contiguous" + int J_r_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* rt, // (6,) vector + int rt_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Invert an Rt transformation +// +// The input is given in Rt_in in a (4,3) array +// +// The result is returned in a (4,3) array Rt_out +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_invert_Rt(Rt_out,Rt_in) mrcal_invert_Rt_full(Rt_out,0,0,Rt_in,0,0) +void mrcal_invert_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_in, // (4,3) array + int Rt_in_stride0, // in bytes. <= 0 means "contiguous" + int Rt_in_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Invert an rt transformation +// +// The input is given in rt_in in a (6,) array +// +// The result is returned in a (6,) array rt_out +// +// The gradient dtout/drin is returned in a (3,3) array dtout_drin. Set to NULL +// if this is not wanted +// +// The gradient dtout/dtin is returned in a (3,3) array dtout_dtin. Set to NULL +// if this is not wanted +// +// The gradient drout/drin is always -identity. So it is not returned +// +// The gradient drout/dtin is always 0. So it is not returned +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_invert_rt(rt_out,dtout_drin,dtout_dtin,rt_in) mrcal_invert_rt_full(rt_out,0,dtout_drin,0,0,dtout_dtin,0,0,rt_in,0) +void mrcal_invert_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dtout_drin, // (3,3) array + int dtout_drin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_drin_stride1, // in bytes. <= 0 means "contiguous" + double* dtout_dtin, // (3,3) array + int dtout_dtin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_dtin_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_in, // (6,) array + int rt_in_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Compose two Rt transformations +// +// Rt = Rt0 * Rt1 ---> Rt(x) = Rt0( Rt1(x) ) +// +// The input transformations are given in (4,3) arrays Rt_0 and Rt_1 +// +// The result is returned in a (4,3) array Rt_out +// +// In-place operation is supported; the output array may be the same as either +// of the input arrays to overwrite the input. +#define mrcal_compose_Rt(Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0) +void mrcal_compose_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_0, // (4,3) array + int Rt_0_stride0, // in bytes. <= 0 means "contiguous" + int Rt_0_stride1, // in bytes. <= 0 means "contiguous" + const double* Rt_1, // (4,3) array + int Rt_1_stride0, // in bytes. <= 0 means "contiguous" + int Rt_1_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Compose two rt transformations +// +// rt = rt0 * rt1 ---> rt(x) = rt0( rt1(x) ) +// +// The input transformations are given in (6,) arrays rt_0 and rt_1 +// +// The result is returned in a (6,) array rt_out +// +// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this +// is not wanted +// +// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this +// is not wanted +// +// The gradient dt/dr0 is returned in a (3,3) array dt_dr0. Set to NULL if this +// is not wanted +// +// The gradient dt/dt1 is returned in a (3,3) array dt_dt1. Set to NULL if this +// is not wanted +// +// The gradients dr/dt0, dr/dt1, dt/dr1 are always 0, so they are never returned +// +// The gradient dt/dt0 is always identity, so it is never returned +// +// In-place operation is supported; the output array may be the same as either +// of the input arrays to overwrite the input. +#define mrcal_compose_rt(rt_out,dr_dr0,dr_dr1,dt_dr0,dt_dt1,rt_0,rt_1) mrcal_compose_rt_full(rt_out,0,dr_dr0,0,0,dr_dr1,0,0,dt_dr0,0,0,dt_dt1,0,0,rt_0,0,rt_1,0) +void mrcal_compose_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + double* dr_dr1, // (3,3) array; may be NULL + int dr_dr1_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr1_stride1, // in bytes. <= 0 means "contiguous" + double* dt_dr0, // (3,3) array; may be NULL + int dt_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dt_dr0_stride1, // in bytes. <= 0 means "contiguous" + double* dt_dt1, // (3,3) array; may be NULL + int dt_dt1_stride0, // in bytes. <= 0 means "contiguous" + int dt_dt1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_0, // (6,) array + int rt_0_stride0, // in bytes. <= 0 means "contiguous" + const double* rt_1, // (6,) array + int rt_1_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Compose two angle-axis rotations +// +// r = r0 * r1 ---> r(x) = r0( r1(x) ) +// +// The input rotations are given in (3,) arrays r_0 and r_1 +// +// The result is returned in a (3,) array r_out +// +// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this +// is not wanted +// +// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this +// is not wanted +// +// In-place operation is supported; the output array may be the same as either +// of the input arrays to overwrite the input. +#define mrcal_compose_r(r_out,dr_dr0,dr_dr1,r_0,r_1) mrcal_compose_r_full(r_out,0,dr_dr0,0,0,dr_dr1,0,0,r_0,0,r_1,0) +void mrcal_compose_r_full( // output + double* r_out, // (3,) array + int r_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + double* dr_dr1, // (3,3) array; may be NULL + int dr_dr1_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r_0, // (3,) array + int r_0_stride0, // in bytes. <= 0 means "contiguous" + const double* r_1, // (3,) array + int r_1_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Special-case rotation compositions for the uncertainty computation +// +// Same as mrcal_compose_r() except +// +// - r0 is assumed to be 0, so we don't ingest it, and we don't report the +// composition result +// - we ONLY report the dr01/dr0 gradient +// +// In python this function is equivalent to +// +// _,dr01_dr0,_ = compose_r(np.zeros((3,),), +// r1, +// get_gradients=True) +#define mrcal_compose_r_tinyr0_gradientr0(dr_dr0,r_1) \ + mrcal_compose_r_tinyr0_gradientr0_full(dr_dr0,0,0,r_1,0) +void mrcal_compose_r_tinyr0_gradientr0_full( // output + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r_1, // (3,) array + int r_1_stride0 // in bytes. <= 0 means "contiguous" + ); +// Same as mrcal_compose_r() except +// +// - r1 is assumed to be 0, so we don't ingest it, and we don't report the +// composition result +// - we ONLY report the dr01/dr1 gradient +// +// In python this function is equivalent to +// +// _,_,dr01_dr1 = compose_r(r0, +// np.zeros((3,),), +// get_gradients=True) +#define mrcal_compose_r_tinyr1_gradientr1(dr_dr1,r_0) \ + mrcal_compose_r_tinyr1_gradientr1_full(dr_dr1,0,0,r_0,0) +void mrcal_compose_r_tinyr1_gradientr1_full( // output + double* dr_dr1, // (3,3) array; may be NULL + int dr_dr1_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r_0, // (3,) array + int r_0_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Procrustes fit functions. Align two corresponding sets of normalized +// direction vectors or points. Return true on success +bool mrcal_align_procrustes_vectors_R01(// out + double* R01, + // in + const int N, + // (N,3) arrays + // normalized direction vectors + const double* v0, + const double* v1, + + // (N,) array; may be NULL to use an even + // weighting + const double* weights); + +bool mrcal_align_procrustes_points_Rt01(// out + double* Rt01, + // in + const int N, + // (N,3) arrays + const double* p0, + const double* p1, + + // (N,) array; may be NULL to use an even + // weighting + const double* weights); + +// Compute a non-unique rotation to map a given vector to [0,0,1] +void mrcal_R_aligned_to_vector(// out + double* R, + // in + const double* v); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/scales.h b/wpical/src/main/native/thirdparty/mrcal/include/scales.h new file mode 100644 index 0000000000..14ed88ab38 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/scales.h @@ -0,0 +1,43 @@ +#pragma once + +// These are parameter variable scales. They have the units of the parameters +// themselves, so the optimizer sees x/SCALE_X for each parameter. I.e. as far +// as the optimizer is concerned, the scale of each variable is 1. This doesn't +// need to be precise; just need to get all the variables to be within the same +// order of magnitute. This is important because the dogleg solve treats the +// trust region as a ball in state space, and this ball is isotropic, and has a +// radius that applies in every direction +// +// Can be visualized like this: +// +// b0,x0,J0 = mrcal.optimizer_callback(**optimization_inputs)[:3] +// J0 = J0.toarray() +// ss = np.sum(np.abs(J0), axis=-2) +// gp.plot(ss, _set=mrcal.plotoptions_state_boundaries(**optimization_inputs)) +// +// This visualizes the overall effect of each variable. If the scales aren't +// tuned properly, some variables will have orders of magnitude stronger +// response than others, and the optimization problem won't converge well. +// +// The scipy.optimize.least_squares() function claims to be able to estimate +// these automatically, without requiring these hard-coded values from the user. +// See the description of the "x_scale" argument: +// +// https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.least_squares.html +// +// Supposedly this paper describes the method: +// +// J. J. More, "The Levenberg-Marquardt Algorithm: Implementation and Theory," +// Numerical Analysis, ed. G. A. Watson, Lecture Notes in Mathematics 630, +// Springer Verlag, pp. 105-116, 1977. +// +// Please somebody look at this +#define SCALE_INTRINSICS_FOCAL_LENGTH 500.0 +#define SCALE_INTRINSICS_CENTER_PIXEL 20.0 +#define SCALE_ROTATION_CAMERA (0.1 * M_PI/180.0) +#define SCALE_TRANSLATION_CAMERA 1.0 +#define SCALE_ROTATION_FRAME (15.0 * M_PI/180.0) +#define SCALE_TRANSLATION_FRAME 1.0 +#define SCALE_POSITION_POINT SCALE_TRANSLATION_FRAME +#define SCALE_CALOBJECT_WARP 0.01 +#define SCALE_DISTORTION 1.0 diff --git a/wpical/src/main/native/thirdparty/mrcal/include/stereo.h b/wpical/src/main/native/thirdparty/mrcal/include/stereo.h new file mode 100644 index 0000000000..793523ab98 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/stereo.h @@ -0,0 +1,129 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +#include "mrcal-types.h" +#include "mrcal-image.h" + +// The reference implementation in Python is _rectified_resolution_python() in +// stereo.py +// +// The Python wrapper is mrcal.rectified_resolution(), and the documentation is +// in the docstring of that function +bool mrcal_rectified_resolution( // output and input + // > 0: use given value + // < 0: autodetect and scale + double* pixels_per_deg_az, + double* pixels_per_deg_el, + + // input + const mrcal_lensmodel_t* lensmodel, + const double* intrinsics, + const mrcal_point2_t* azel_fov_deg, + const mrcal_point2_t* azel0_deg, + const double* R_cam0_rect0, + const mrcal_lensmodel_type_t rectification_model_type); + +// The reference implementation in Python is _rectified_system_python() in +// stereo.py +// +// The Python wrapper is mrcal.rectified_system(), and the documentation is in +// the docstring of that function +bool mrcal_rectified_system(// output + unsigned int* imagersize_rectified, + double* fxycxy_rectified, + double* rt_rect0_ref, + double* baseline, + + // input, output + // > 0: use given value + // < 0: autodetect and scale + double* pixels_per_deg_az, + double* pixels_per_deg_el, + + // input, output + // if(..._autodetect) { the results are returned here } + mrcal_point2_t* azel_fov_deg, + mrcal_point2_t* azel0_deg, + + // input + const mrcal_lensmodel_t* lensmodel0, + const double* intrinsics0, + + const double* rt_cam0_ref, + const double* rt_cam1_ref, + + const mrcal_lensmodel_type_t rectification_model_type, + + bool az0_deg_autodetect, + bool el0_deg_autodetect, + bool az_fov_deg_autodetect, + bool el_fov_deg_autodetect); + +// The reference implementation in Python is _rectification_maps_python() in +// stereo.py +// +// The Python wrapper is mrcal.rectification_maps(), and the documentation is in +// the docstring of that function +bool mrcal_rectification_maps(// output + // Dense array of shape (Ncameras=2, Nel, Naz, Nxy=2) + float* rectification_maps, + + // input + const mrcal_lensmodel_t* lensmodel0, + const double* intrinsics0, + const double* r_cam0_ref, + + const mrcal_lensmodel_t* lensmodel1, + const double* intrinsics1, + const double* r_cam1_ref, + + const mrcal_lensmodel_type_t rectification_model_type, + const double* fxycxy_rectified, + const unsigned int* imagersize_rectified, + const double* r_rect0_ref); + + +// The reference implementation in Python is _stereo_range_python() in +// stereo.py +// +// The Python wrapper is mrcal.stereo_range(), and the documentation is in +// the docstring of that function +bool mrcal_stereo_range_sparse(// output + double* range, // N of these + + // input + const double* disparity, // N of these + const mrcal_point2_t* qrect0, // N of these + const int N, // how many points + + const double disparity_min, + const double disparity_max, + + // models_rectified + const mrcal_lensmodel_type_t rectification_model_type, + const double* fxycxy_rectified, + const double baseline); + +bool mrcal_stereo_range_dense(// output + mrcal_image_double_t* range, + + // input + const mrcal_image_uint16_t* disparity_scaled, + const uint16_t disparity_scale, + + // Used to detect invalid values. Set to + // 0,UINT16_MAX to ignore + const uint16_t disparity_scaled_min, + const uint16_t disparity_scaled_max, + + // models_rectified + const mrcal_lensmodel_type_t rectification_model_type, + const double* fxycxy_rectified, + const double baseline); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/strides.h b/wpical/src/main/native/thirdparty/mrcal/include/strides.h new file mode 100644 index 0000000000..2c8ebf27f2 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/strides.h @@ -0,0 +1,42 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +// This is an internal header to make the stride logic work. Not to be seen by +// the end-users or installed + +// stride-aware matrix access +#define _P1(x, stride0, i0) \ + (*(double*)( (char*)(x) + \ + (i0) * (stride0))) +#define _P2(x, stride0, stride1, i0, i1) \ + (*(double*)( (char*)(x) + \ + (i0) * (stride0) + \ + (i1) * (stride1))) +#define _P3(x, stride0, stride1, stride2,i0, i1, i2) \ + (*(double*)( (char*)(x) + \ + (i0) * (stride0) + \ + (i1) * (stride1) + \ + (i2) * (stride2))) + +#define P1(x, i0) _P1(x, x##_stride0, i0) +#define P2(x, i0,i1) _P2(x, x##_stride0, x##_stride1, i0,i1) +#define P3(x, i0,i1,i2) _P3(x, x##_stride0, x##_stride1, x##_stride2, i0,i1,i2) + +// Init strides. If a given stride is <= 0, set the default, as we would have if +// the data was contiguous +#define init_stride_1D(x, d0) \ + if( x ## _stride0 <= 0) x ## _stride0 = sizeof(*x) +#define init_stride_2D(x, d0, d1) \ + if( x ## _stride1 <= 0) x ## _stride1 = sizeof(*x); \ + if( x ## _stride0 <= 0) x ## _stride0 = d1 * x ## _stride1 +#define init_stride_3D(x, d0, d1, d2) \ + if( x ## _stride2 <= 0) x ## _stride2 = sizeof(*x); \ + if( x ## _stride1 <= 0) x ## _stride1 = d2 * x ## _stride2; \ + if( x ## _stride0 <= 0) x ## _stride0 = d1 * x ## _stride1 diff --git a/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h b/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h new file mode 100644 index 0000000000..cb685f9500 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h @@ -0,0 +1,153 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +#include "basic-geometry.h" + +// All of these return (0,0,0) if the rays are parallel or divergent, or if the +// intersection is behind either of the two cameras. No gradients are reported +// in that case + +// Basic closest-approach-in-3D routine. This is the "Mid" method in +// "Triangulation: Why Optimize?", Seong Hun Lee and Javier Civera +// https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_geometric(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* v0, + const mrcal_point3_t* v1, + const mrcal_point3_t* t01); + +// Minimize L2 pinhole reprojection error. Described in "Triangulation Made +// Easy", Peter Lindstrom, IEEE Conference on Computer Vision and Pattern +// Recognition, 2010. This is the "L2 img 5-iteration" method (but with only 2 +// iterations) in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +// Lindstrom's paper recommends 2 iterations +mrcal_point3_t +mrcal_triangulate_lindstrom(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_dRt01, + + // inputs + + // not-necessarily normalized vectors in the LOCAL + // coordinate system. This is different from the other + // triangulation routines + const mrcal_point3_t* v0_local, + const mrcal_point3_t* v1_local, + const mrcal_point3_t* Rt01); + +// Minimize L1 angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. This is the "L1 ang" method in "Triangulation: Why Optimize?", Seong +// Hun Lee and Javier Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_l1(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* v0, + const mrcal_point3_t* v1, + const mrcal_point3_t* t01); + +// Minimize L-infinity angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. This is the "L-infinity ang" method in "Triangulation: Why Optimize?", +// Seong Hun Lee and Javier Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_linf(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* v0, + const mrcal_point3_t* v1, + const mrcal_point3_t* t01); + +// The "Mid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_mid2(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* v0, + const mrcal_point3_t* v1, + const mrcal_point3_t* t01); + +// The "wMid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_wmid2(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* v0, + const mrcal_point3_t* v1, + const mrcal_point3_t* t01); + +// I don't implement triangulate_leecivera_l2() yet because it requires +// computing an SVD, which is far slower than what the rest of these functions +// do + +double +_mrcal_triangulated_error(// outputs + mrcal_point3_t* _derr_dv1, + mrcal_point3_t* _derr_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01); + +bool +_mrcal_triangulate_leecivera_mid2_is_convergent(// inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/util.h b/wpical/src/main/native/thirdparty/mrcal/include/util.h new file mode 100644 index 0000000000..bd151891ef --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/util.h @@ -0,0 +1,13 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#pragma once + +#include + +#define MSG(fmt, ...) fprintf(stderr, "%s(%d): " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__) diff --git a/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp b/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp new file mode 100644 index 0000000000..1717c3e784 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp @@ -0,0 +1,332 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#include +#include + +#include "autodiff.hh" + +extern "C" { +#include "cahvore.h" +} + +template +static +bool _project_cahvore_internals( // outputs + vec_withgrad_t* pdistorted, + + // inputs + const vec_withgrad_t& p, + const val_withgrad_t& alpha, + const val_withgrad_t& beta, + const val_withgrad_t& r0, + const val_withgrad_t& r1, + const val_withgrad_t& r2, + const val_withgrad_t& e0, + const val_withgrad_t& e1, + const val_withgrad_t& e2, + const double cahvore_linearity) +{ + // Apply a CAHVORE warp to an un-distorted point + + // Given intrinsic parameters of a CAHVORE model and a set of + // camera-coordinate points, return the projected point(s) + + // This comes from cmod_cahvore_3d_to_2d_general() in + // m-jplv/libcmod/cmod_cahvore.c + // + // The lack of documentation here comes directly from the lack of + // documentation in that function. + + // I parametrize the optical axis such that + // - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center + // if both parameters are 0 + // - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both + // NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal + // lock), and that would make my solver unhappy + // So o = { s_al*c_be, s_be, c_al*c_be } + vec_withgrad_t sca = alpha.sincos(); + vec_withgrad_t scb = beta .sincos(); + + vec_withgrad_t o; + o[0] = scb[1]*sca[0]; + o[1] = scb[0]; + o[2] = scb[1]*sca[1]; + + // Note: CAHVORE is noncentral: project(p) and project(k*p) do NOT + // project to the same point + + // What is called "omega" in the canonical CAHVOR implementation is + // called "zeta" in the canonical CAHVORE implementation. They're the + // same thing + + // cos( angle between p and o ) = inner(p,o) / (norm(o) * norm(p)) = + // zeta/norm(p) + val_withgrad_t zeta = p.dot(o); + + // Basic Computations + // Calculate initial terms + vec_withgrad_t u = o*zeta; + vec_withgrad_t ll = p - u; + val_withgrad_t l = ll.mag(); + + // Calculate theta using Newton's Method + val_withgrad_t theta = l.atan2(zeta); + + int inewton; + for( inewton = 100; inewton; inewton--) + { + // Compute terms from the current value of theta + vec_withgrad_t scth = theta.sincos(); + + val_withgrad_t theta2 = theta * theta; + val_withgrad_t theta3 = theta * theta2; + val_withgrad_t theta4 = theta * theta3; + val_withgrad_t upsilon = + zeta*scth[1] + l*scth[0] + + (scth[1] - 1.0 ) * (e0 + e1*theta2 + e2*theta4) + - (theta - scth[0]) * ( e1*theta*2.0 + e2*theta3*4.0); + + // Update theta + val_withgrad_t dtheta = + (zeta*scth[0] - l*scth[1] + - (theta - scth[0]) * (e0 + e1*theta2 + e2*theta4)) / upsilon; + + theta -= dtheta; + + // Check exit criterion from last update + if(fabs(dtheta.x) < 1e-8) + break; + } + if(inewton == 0) + { + fprintf(stderr, "%s(): too many iterations\n", __func__); + return false; + } + + // got a theta + + // Check the value of theta + if(theta.x * fabs(cahvore_linearity) > M_PI/2.) + { + fprintf(stderr, "%s(): theta out of bounds\n", __func__); + return false; + } + + // If we aren't close enough to use the small-angle approximation ... + if (theta.x > 1e-8) + { + // ... do more math! + val_withgrad_t linth = theta * cahvore_linearity; + val_withgrad_t chi; + if (cahvore_linearity < -1e-15) + chi = linth.sin() / cahvore_linearity; + else if (cahvore_linearity > 1e-15) + chi = linth.tan() / cahvore_linearity; + else + chi = theta; + val_withgrad_t chi2 = chi * chi; + val_withgrad_t chi3 = chi * chi2; + val_withgrad_t chi4 = chi * chi3; + + val_withgrad_t zetap = l / chi; + + val_withgrad_t mu = r0 + r1*chi2 + r2*chi4; + + vec_withgrad_t uu = o*zetap; + vec_withgrad_t vv = ll * (mu + 1.); + *pdistorted = uu + vv; + } + else + *pdistorted = p; + return true; +} + + +// Not meant to be touched by the end user. Implemented separate from mrcal.c so +// that I can get automated gradient propagation with c++ +extern "C" + +bool project_cahvore_internals( // outputs + mrcal_point3_t* __restrict pdistorted, + double* __restrict dpdistorted_dintrinsics_nocore, + double* __restrict dpdistorted_dp, + + // inputs + const mrcal_point3_t* __restrict p, + const double* __restrict intrinsics_nocore, + const double cahvore_linearity) +{ + if( dpdistorted_dintrinsics_nocore == NULL && + dpdistorted_dp == NULL ) + { + // No gradients. NGRAD in all the templates is 0 + + vec_withgrad_t<0,3> pdistortedg; + + vec_withgrad_t<0,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + -1 // i_gradvec0 + ); + + vec_withgrad_t<0,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + -1 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + return true; + } + + if( dpdistorted_dintrinsics_nocore == NULL && + dpdistorted_dp != NULL ) + { + // 3 variables: p (3 vars) + vec_withgrad_t<3,3> pdistortedg; + + vec_withgrad_t<3,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + -1 // i_gradvec0 + ); + + vec_withgrad_t<3,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + 0 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + pdistortedg.extract_grad (dpdistorted_dp, + 0,3, // ivar0,Nvars + 0, // i_gradvec0 + sizeof(double)*3, sizeof(double), + 3 // Nvars + ); + return true; + } + + if( dpdistorted_dintrinsics_nocore != NULL && + dpdistorted_dp == NULL ) + { + // 8 variables: alpha,beta,R,E (8 vars) + vec_withgrad_t<8,3> pdistortedg; + + vec_withgrad_t<8,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + 0 // i_gradvec0 + ); + + vec_withgrad_t<8,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + -1 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore, + 0,8, // i_gradvec0,N_gradout + 0, // ivar0 + sizeof(double)*8, sizeof(double), + 3 // Nvars + ); + return true; + } + + if( dpdistorted_dintrinsics_nocore != NULL && + dpdistorted_dp != NULL ) + { + // 11 variables: alpha,beta,R,E (8 vars) + p (3 vars) + vec_withgrad_t<11,3> pdistortedg; + + vec_withgrad_t<11,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + 0 // i_gradvec0 + ); + + vec_withgrad_t<11,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + 8 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore, + 0,8, // i_gradvec0,N_gradout + 0, // ivar0 + sizeof(double)*8, sizeof(double), + 3 // Nvars + ); + pdistortedg.extract_grad (dpdistorted_dp, + 8,3, // ivar0,Nvars + 0, // i_gradvec0 + sizeof(double)*3, sizeof(double), + 3 // Nvars + ); + return true; + } + + fprintf(stderr, "Getting here is a bug. Please report\n"); + assert(0); +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl b/wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl new file mode 100755 index 0000000000..044098b9f2 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl @@ -0,0 +1,463 @@ +#!/usr/bin/env perl +use strict; +use warnings; +use feature qw(say); +use List::Util qw(min); +use List::MoreUtils qw(pairwise); + +say "// THIS IS AUTO-GENERATED BY $0. DO NOT EDIT BY HAND\n"; +say "// This contains dot products, norms, basic vector arithmetic and multiplication\n"; + +my @sizes = 2..6; + +# the dot products, norms and basic arithmetic functions take the size as an +# argument. I'm assuming that the compiler will expand these out for each +# particular invocation +dotProducts(); +norms(); +vectorArithmetic(); + +foreach my $n(@sizes) +{ + matrixVectorSym($n); + + foreach my $m (@sizes) + { + matrixVectorGen($n, $m) + } + + matrixMatrixSym($n); + matrixMatrixGen($n); +} + +# this is only defined for N=3. I haven't made the others yet and I don't yet need them +matrixMatrixMatrixSym(3); + + + + + + +sub dotProducts +{ + print < vout +static inline void add_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i vout +static inline void sub_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i 0); + + for my $i(0..$n-1) + { + my $isym_row = _getSymmetricIndices_row(\%isymHash, $i, $n); + my @cols = 0..$n-1; + + our ($a,$b); + my @sum_components = pairwise {"s[$a]*v[$b]"} @$isym_row, @cols; + $vout .= " vout[$i] = " . join(' + ', @sum_components) . ";\n"; + } + + $vout .= "}"; + + + print _multiplicationVersions($vout, $n, $n); +} + + +sub matrixVectorGen +{ + my $n = shift; + my $m = shift; + + # I now make NxM matrix-vector multiplication. I describe matrices math-style + # with the number of rows first (NxM has N rows, M columns). I store the + # matrices row-first and treat vectors as row-vectors. Thus these functons + # compute v*A where v is the row vector and A is the NxM matrix + + my $vout = <{$key} ) + { + $hash->{$key} = $hash->{next}; + $hash->{next}++; + } + + push @isym, $hash->{$key}; + } + + return \@isym; +} + +sub _getFirstDataArg +{ + my $v = shift; + + # I have a string with a bunch of functions. Get the first argument. I ASSUME + # THE FIRST ARGUMENT HAS THE SAME NAME IN ALL OF THESE + my ($arg0) = $v =~ m/^static inline.*\(.*?double\* restrict ([a-z0-9_]+),/m or die "Couldn't recognize function in '$v'"; + return $arg0; +} + +sub _makeInplace_mulVector +{ + my $v = shift; + my $arg0 = shift; + my $n = shift; + my $m = shift; + + # rename functions + $v =~ s/_vout//gm; + + # get rid of the 'vout argument' + $v =~ s/, double\* restrict vout//gm; + + # un-const first argument + $v =~ s/^(static inline.*\(.*?)const (double.*)$/$1$2/gm; + + # use the first argument instead of vout + $v =~ s/vout/$arg0/gm; + + # if we're asked to make some temporary variables, do it + if(defined $n) + { + # if no $m is given, use $m; + $m //= $n; + + my $nt = min($n-1,$m); + + # use the temporaries instead of the main variable when possible + foreach my $t(0..$nt-1) + { + $v =~ s/(=.*)${arg0}\[$t\]/$1t[$t]/mg; + } + + # define the temporaries. I need one fewer than n + my $tempDef = " double t[$nt] = {" . join(', ', map {"${arg0}[$_]"} 0..$nt-1) . "};"; + $v =~ s/^\{$/{\n$tempDef/mg; + } + + return $v; +} +sub _makeInplace_mulMatrix +{ + my $v = shift; + + # rename functions + $v =~ s/_vout//gm; + + # get rid of the 'vout argument' + $v =~ s/, double\* restrict vout//gm; + + # un-const first argument + $v =~ s/^(static inline.*\(.*?)const (double.*)$/$1$2/gm; + + # use the first argument instead of vout + $v =~ s/,[^\),]*vout[^\),]*([\),])/$1/gm; + + return $v; +} + +sub _makeVaccum +{ + my $v = shift; + + # rename functions + $v =~ s/_vout/_vaccum/gm; + + # vout -> vaccum + $v =~ s/vout/vaccum/gm; + + # make sure we accumulate + $v =~ s/(vaccum\[.*?\]\s*)=/$1+=/gm; + + # better comment + $v =~ s/-> vaccum/-> + vaccum/gm; + + return $v; +} + +sub _makeScaled_arithmetic +{ + my $f = shift; + + # rename functions + $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm; + + # add the scale argument + $f =~ s/^(static inline .*)\)$/$1, double scale)/gm; + + # apply the scaling + $f =~ s/([+-]) b/$1 scale*b/gm; + + return $f; +} + +sub _makeScaled_mulVector +{ + my $f = shift; + + # rename functions + $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm; + + # add the scale argument + $f =~ s/^(static inline .*)\)$/$1, double scale)/gm; + + # apply the scaling + $f =~ s/(.*=\s*)([^{}]*?);$/${1}scale * ($2);/gm; + + return $f; +} + +sub _makeScaled_mulMatrix +{ + my $f = shift; + + # rename functions + $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm; + + # add the scale argument + $f =~ s/^(static inline .*)\)$/$1, double scale)/gm; + + # apply the scaling. This is simply an argument to the vector function I call + $f =~ s/^(\s*mul_.*)(\).*)/$1, scale$2/gm; + + # apply the scaling. Call the _scaled vector function + $f =~ s/^(\s*mul_.*?)(\s*\()/${1}_scaled$2/gm; + + return $f; +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/mrcal.cpp b/wpical/src/main/native/thirdparty/mrcal/src/mrcal.cpp new file mode 100644 index 0000000000..5ee9337e33 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/mrcal.cpp @@ -0,0 +1,6890 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +// Apparently I need this in MSVC to get constants +#include +#define _USE_MATH_DEFINES + +#include +#include +#include + +extern "C" { +#include +} +#include +#include +#include +#include + +extern "C" { +#include "mrcal.h" +#include "minimath/minimath.h" +#include "cahvore.h" +#include "minimath/minimath-extra.h" +#include "util.h" +#include "scales.h" +} +#define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0) + +#define restrict __restrict + +// Returns a static string, using "..." as a placeholder for any configuration +// values +#define LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + "_" #name "=..." +#define LENSMODEL_PRINT_CFG_ELEMENT_FMT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + "_" #name "=%" PRIcode +#define LENSMODEL_PRINT_CFG_ELEMENT_VAR(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + ,config->name +#define LENSMODEL_SCAN_CFG_ELEMENT_FMT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + "_" #name "=%" SCNcode +#define LENSMODEL_SCAN_CFG_ELEMENT_VAR(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + ,&config->name +#define LENSMODEL_SCAN_CFG_ELEMENT_PLUS1(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + +1 +const char* mrcal_lensmodel_name_unconfigured( const mrcal_lensmodel_t* lensmodel ) +{ + switch(lensmodel->type) + { +#define CASE_STRING_NOCONFIG(s,n) case MRCAL_##s: ; \ + return #s; +#define _CASE_STRING_WITHCONFIG(s,n,s_CONFIG_LIST) case MRCAL_##s: ; \ + return #s s_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE, ); +#define CASE_STRING_WITHCONFIG(s,n) _CASE_STRING_WITHCONFIG(s,n,MRCAL_ ## s ## _CONFIG_LIST) + + MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_STRING_NOCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + + default: + return NULL; + + +#undef CASE_STRING_NOCONFIG +#undef CASE_STRING_WITHCONFIG + + } + return NULL; +} + +// Write the model name WITH the full config into the given buffer. Identical to +// mrcal_lensmodel_name_unconfigured() for configuration-free models +static int LENSMODEL_CAHVORE__snprintf_model + (char* out, int size, + const mrcal_LENSMODEL_CAHVORE__config_t* config) +{ + return + snprintf( out, size, "LENSMODEL_CAHVORE" + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_FMT, ) + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_VAR, )); +} +static int LENSMODEL_SPLINED_STEREOGRAPHIC__snprintf_model + (char* out, int size, + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config) +{ + return + snprintf( out, size, "LENSMODEL_SPLINED_STEREOGRAPHIC" + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_FMT, ) + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_VAR, )); +} +bool mrcal_lensmodel_name( char* out, int size, const mrcal_lensmodel_t* lensmodel ) +{ + switch(lensmodel->type) + { +#define CASE_STRING_NOCONFIG(s,n) case MRCAL_##s: \ + return size > snprintf(out,size, #s); + +#define CASE_STRING_WITHCONFIG(s,n) case MRCAL_##s: \ + return size > s##__snprintf_model(out, size, &lensmodel->s##__config); + + MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_STRING_NOCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + + default: + return NULL; + +#undef CASE_STRING_NOCONFIG +#undef CASE_STRING_WITHCONFIG + + } + return NULL; +} + + +static bool LENSMODEL_CAHVORE__scan_model_config( mrcal_LENSMODEL_CAHVORE__config_t* config, const char* config_str) +{ + int pos; + int Nelements = 0 MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_PLUS1, ); + return + Nelements == + sscanf( config_str, + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_FMT, )"%n" + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_VAR, ), + &pos) && + config_str[pos] == '\0'; +} +static bool LENSMODEL_SPLINED_STEREOGRAPHIC__scan_model_config( mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config, const char* config_str) +{ + int pos; + int Nelements = 0 MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_PLUS1, ); + return + Nelements == + sscanf( config_str, + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_FMT, )"%n" + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_VAR, ), + &pos) && + config_str[pos] == '\0'; +} + +const char* const* mrcal_supported_lensmodel_names( void ) +{ +#define NAMESTRING_NOCONFIG(s,n) #s, +#define _NAMESTRING_WITHCONFIG(s,n,s_CONFIG_LIST) #s s_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE, ), +#define NAMESTRING_WITHCONFIG(s,n) _NAMESTRING_WITHCONFIG(s,n,MRCAL_ ## s ## _CONFIG_LIST) + + static const char* names[] = { + MRCAL_LENSMODEL_NOCONFIG_LIST( NAMESTRING_NOCONFIG) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( NAMESTRING_WITHCONFIG) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(NAMESTRING_WITHCONFIG) + NULL }; + return names; +} + +#undef LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE +#undef LENSMODEL_PRINT_CFG_ELEMENT_FMT +#undef LENSMODEL_PRINT_CFG_ELEMENT_VAR +#undef LENSMODEL_SCAN_CFG_ELEMENT_FMT +#undef LENSMODEL_SCAN_CFG_ELEMENT_VAR +#undef LENSMODEL_SCAN_CFG_ELEMENT_PLUS1 + +// parses the model name AND the configuration into a mrcal_lensmodel_t structure. +// On error returns false with lensmodel->type set to MRCAL_LENSMODEL_INVALID_... +bool mrcal_lensmodel_from_name( // output + mrcal_lensmodel_t* lensmodel, + + // input + const char* name ) +{ +#define CHECK_AND_RETURN_NOCONFIG(s,n) \ + if( 0 == strcmp( name, #s) ) \ + { \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_##s}; \ + return true; \ + } + +#define CHECK_AND_RETURN_WITHCONFIG(s,n) \ + /* Configured model. I need to extract the config from the string. */ \ + /* The string format is NAME_cfg1=var1_cfg2=var2... */ \ + { \ + const int len_s = strlen(#s); \ + if( 0 == strncmp( name, #s, len_s ) ) \ + { \ + if(name[len_s] == '\0') \ + { \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_MISSINGCONFIG}; \ + return false; \ + } \ + if(name[len_s] == '_') \ + { \ + /* found name. Now extract the config */ \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_##s}; \ + mrcal_##s##__config_t* config = &lensmodel->s##__config; \ + \ + const char* config_str = &name[len_s]; \ + \ + if(s##__scan_model_config(config, config_str)) \ + return true; \ + \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_BADCONFIG}; \ + return false; \ + } \ + } \ + } + + if(name == NULL) + { + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_TYPE}; + return false; + } + + MRCAL_LENSMODEL_NOCONFIG_LIST( CHECK_AND_RETURN_NOCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_TYPE}; + return false; +#undef CHECK_AND_RETURN_NOCONFIG +#undef CHECK_AND_RETURN_WITHCONFIG +} + +// parses the model name only. The configuration is ignored. Even if it's +// missing or unparseable. Unknown model names return +// MRCAL_LENSMODEL_INVALID_TYPE +mrcal_lensmodel_type_t mrcal_lensmodel_type_from_name( const char* name ) +{ +#define CHECK_AND_RETURN_NOCONFIG(s,n) \ + if( 0 == strcmp( name, #s) ) return MRCAL_##s; + +#define CHECK_AND_RETURN_WITHCONFIG(s,n) \ + /* Configured model. If the name is followed by _ or nothing, I */ \ + /* accept this model */ \ + { \ + const int len_s = strlen(#s); \ + if( 0 == strncmp( name, #s, len_s) && \ + ( name[len_s] == '\0' || \ + name[len_s] == '_' ) ) \ + return MRCAL_##s; \ + } + + if(name == NULL) + return MRCAL_LENSMODEL_INVALID_TYPE; + + MRCAL_LENSMODEL_NOCONFIG_LIST( CHECK_AND_RETURN_NOCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + + return MRCAL_LENSMODEL_INVALID_TYPE; + +#undef CHECK_AND_RETURN_NOCONFIG +#undef CHECK_AND_RETURN_WITHCONFIG +} + +mrcal_lensmodel_metadata_t mrcal_lensmodel_metadata( const mrcal_lensmodel_t* lensmodel ) +{ + switch(lensmodel->type) + { + case MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC: + case MRCAL_LENSMODEL_STEREOGRAPHIC: + case MRCAL_LENSMODEL_LONLAT: + case MRCAL_LENSMODEL_LATLON: + return (mrcal_lensmodel_metadata_t) { .has_core = true, + .can_project_behind_camera = true, + .has_gradients = true, + .noncentral = false}; + case MRCAL_LENSMODEL_PINHOLE: + case MRCAL_LENSMODEL_OPENCV4: + case MRCAL_LENSMODEL_OPENCV5: + case MRCAL_LENSMODEL_OPENCV8: + case MRCAL_LENSMODEL_OPENCV12: + case MRCAL_LENSMODEL_CAHVOR: + return (mrcal_lensmodel_metadata_t) { .has_core = true, + .can_project_behind_camera = false, + .has_gradients = true, + .noncentral = false }; + + case MRCAL_LENSMODEL_CAHVORE: + return (mrcal_lensmodel_metadata_t) { .has_core = true, + .can_project_behind_camera = false, + .has_gradients = true, + .noncentral = true }; + + default: ; + } + MSG("Unknown lens model %d. Barfing out", lensmodel->type); + assert(0); +} + +static +bool modelHasCore_fxfycxcy( const mrcal_lensmodel_t* lensmodel ) +{ + mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(lensmodel); + return meta.has_core; +} +static +bool model_supports_projection_behind_camera( const mrcal_lensmodel_t* lensmodel ) +{ + mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(lensmodel); + return meta.can_project_behind_camera; +} + +static int LENSMODEL_SPLINED_STEREOGRAPHIC__lensmodel_num_params(const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config) +{ + return + // I have two surfaces: one for x and another for y + (int)config->Nx * (int)config->Ny * 2 + + + // and I have a core + 4; +} +int mrcal_lensmodel_num_params(const mrcal_lensmodel_t* lensmodel) +{ +#define CHECK_CONFIG_NPARAM_NOCONFIG(s,n) \ + _Static_assert(n >= 0, "no-config implies known-at-compile-time param count"); + + switch(lensmodel->type) + { +#define CASE_NUM_STATIC(s,n) \ + case MRCAL_##s: return n; + +#define CASE_NUM_DYNAMIC(s,n) \ + case MRCAL_##s: return s##__lensmodel_num_params(&lensmodel->s##__config); + + MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_NUM_STATIC ) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CASE_NUM_STATIC ) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CASE_NUM_DYNAMIC ) + + default: ; + } + return -1; + +#undef CASE_NUM_NOCONFIG +#undef CASE_NUM_WITHCONFIG +} + +static +int get_num_distortions_optimization_params(mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if( !problem_selections.do_optimize_intrinsics_distortions ) + return 0; + + int N = mrcal_lensmodel_num_params(lensmodel); + if(modelHasCore_fxfycxcy(lensmodel)) + N -= 4; // ignoring fx,fy,cx,cy + return N; +} + +int mrcal_num_intrinsics_optimization_params(mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int N = get_num_distortions_optimization_params(problem_selections, lensmodel); + + if( problem_selections.do_optimize_intrinsics_core && + modelHasCore_fxfycxcy(lensmodel) ) + N += 4; // fx,fy,cx,cy + return N; +} + +int mrcal_num_states(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + mrcal_num_states_frames(Nframes, + problem_selections) + + mrcal_num_states_points(Npoints, Npoints_fixed, + problem_selections) + + mrcal_num_states_calobject_warp( problem_selections, + Nobservations_board); +} + +static int num_regularization_terms_percamera(mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(!problem_selections.do_apply_regularization) + return 0; + + // distortions + int N = get_num_distortions_optimization_params(problem_selections, lensmodel); + // optical center + if(problem_selections.do_optimize_intrinsics_core) + N += 2; + return N; +} + +int mrcal_measurement_index_boards(int i_observation_board, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(Nobservations_board <= 0) + return -1; + + // *2 because I have separate x and y measurements + return + 0 + + mrcal_num_measurements_boards(i_observation_board, + calibration_object_width_n, + calibration_object_height_n); +} + +int mrcal_num_measurements_boards(int Nobservations_board, + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(Nobservations_board <= 0) + return 0; + + // *2 because I have separate x and y measurements + return + Nobservations_board * + calibration_object_width_n*calibration_object_height_n * + 2; + + + return mrcal_measurement_index_boards( Nobservations_board, + 0,0, + calibration_object_width_n, + calibration_object_height_n); +} + +int mrcal_measurement_index_points(int i_observation_point, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(Nobservations_point <= 0) + return -1; + + // 2: x,y measurements + return + mrcal_num_measurements_boards(Nobservations_board, + calibration_object_width_n, + calibration_object_height_n) + + i_observation_point * 2; +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: need known-range points, at-infinity points" +#endif + +int mrcal_num_measurements_points(int Nobservations_point) +{ + // 2: x,y measurements + return Nobservations_point * 2; +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: Add a test for mrcal_measurement_index_points_triangulated()" +#endif +int mrcal_measurement_index_points_triangulated(int i_point_triangulated, + int Nobservations_board, + int Nobservations_point, + + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(observations_point_triangulated == NULL || + Nobservations_point_triangulated <= 0) + return -1; + + return + mrcal_num_measurements_boards(Nobservations_board, + calibration_object_width_n, + calibration_object_height_n) + + mrcal_num_measurements_points(Nobservations_point) + + mrcal_num_measurements_points_triangulated_initial_Npoints(observations_point_triangulated, + Nobservations_point_triangulated, + i_point_triangulated); +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: python-wrap this function" +#endif +int mrcal_num_measurements_points_triangulated_initial_Npoints(// May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + // Only consider the leading Npoints. If Npoints < 0: take ALL the points + int Npoints) +{ + if(observations_point_triangulated == NULL || + Nobservations_point_triangulated <= 0) + return 0; + + // Similar loop as in _mrcal_num_j_nonzero(). If the data layout changes, + // update this and that + + int Nmeas = 0; + int ipoint = 0; + int iobservation = 0; + + while( iobservation < Nobservations_point_triangulated && + (Npoints < 0 || ipoint < Npoints)) + { + int Nset = 1; + while(!observations_point_triangulated[iobservation].last_in_set) + { + Nmeas += Nset++; + iobservation++; + } + + ipoint++; + iobservation++; + } + + return Nmeas; +} + +int mrcal_num_measurements_points_triangulated(// May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated) +{ + return + mrcal_num_measurements_points_triangulated_initial_Npoints( observations_point_triangulated, + Nobservations_point_triangulated, + -1 ); +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: python-wrap this function" +#endif +bool mrcal_decode_observation_indices_points_triangulated( + // output + int* iobservation0, + int* iobservation1, + int* iobservation_point0, + int* Nobservations_this_point, + int* Nmeasurements_this_point, + int* ipoint, + + // input + const int imeasurement, + + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated) +{ + if(observations_point_triangulated == NULL || + Nobservations_point_triangulated <= 0) + return false; + + // Similar loop as in + // mrcal_num_measurements_points_triangulated_initial_Npoints(). If the data + // layout changes, update this and that + + int Nmeas = 0; + int iobservation = 0; + *ipoint = 0; + + while( iobservation < Nobservations_point_triangulated ) + { + int Nset = 1; + while(!observations_point_triangulated[iobservation].last_in_set) + { + Nset++; + iobservation++; + } + + // This point has Nset observations. Each pair of observations produces + // a measurement, so: + const int Nmeas_thispoint = Nset*(Nset-1) / 2; + + // Done with this point. It is described by Nmeas_thispoint + // measurements, with the first one at Nmeas. The last observation is at + // iobservation + + if(imeasurement < Nmeas+Nmeas_thispoint) + { + // The measurement I care about observes THIS point. I find the + // specific observations. + // + // I simplify the notation: "m" is "imeasurement", "o" is + // "iobservation". + + const int No = Nset; + const int Nm = Nmeas_thispoint; + const int m = imeasurement - Nmeas; + + // Within this point o is in range(No) and m is in range(Nm). A pair + // (o0,o1) describes one measurement. Both o0 and o1 are in + // range(No) and o1>o0. A sample table of measurement indices m for + // No = 5: + // + // o1 + // 0 1 2 3 4 + // --------- + // 0| 0 1 2 3 + // o0 1| 4 5 6 + // 2| 7 8 + // 3| 9 + // 4| + // + // For a given o0, the maximum m = + // + // m_max = (No-1) + (No-2) + (No-3) ... - 1 + // + // for a total of o0+1 (No...) terms so + // + // m_max = ((No-1) + (No-o0-1))/2 * (o0+1) - 1 + // = (2*No - 2 - o0)/2 * (o0+1) - 1 + // + // m_min = m_max(o0-1) + 1 + // = (2*No - 2 - o0 + 1)/2 * o0 + // = (2*No - 1 - o0) * o0 / 2 + // = (-o0^2 + (2*No - 1)*o0 ) / 2 + // + // -> (-1/2) o0^2 + (2*No - 1)/2 o0 - m_min = 0 + // -> o0 = (2*No - 1)/2 +- sqrt( (2*No - 1)^2/4 - 2*m_min) + // = (2*No - 1)/2 - sqrt( (2*No - 1)^2/4 - 2*m_min) + // = (2*No - 1 - sqrt( (2*No - 1)^2 - 8*m_min))/2 + // + // o0(m) = floor(o0(m)) + // + // Now that I have o0 I compute + // + // o1 = m - m_min(o0) + o0 + 1 + // = m - (-o0^2 + (2*No - 1)*o0 ) / 2 + o0 + 1 + // = m + (o0^2 + (3- 2*No)*o0 + 2) / 2 + // = m + o0*(o0 + 3 - 2*No)/2 + 1 + const double x = 2.*(double)No - 1.; + *iobservation0 = (int)((x - sqrt( x*x - 8.*(double)m))/2.); + *iobservation1 = m + (*iobservation0)*((*iobservation0) + 3 - 2*No)/2 + 1; + + // Reference the observations from the first + *iobservation_point0 = iobservation-Nset+1; + *iobservation0 += *iobservation_point0; + *iobservation1 += *iobservation_point0; + + *Nobservations_this_point = No; + *Nmeasurements_this_point = Nm; + + return true; + } + + Nmeas += Nmeas_thispoint; + + iobservation++; + (*ipoint)++; + } + + return false; +} + + +int mrcal_measurement_index_regularization( +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: this argument order is weird. Put then triangulated stuff at the end?" +#endif + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, int Nobservations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + + if(mrcal_num_measurements_regularization(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel) <= 0) + return -1; + + return + mrcal_num_measurements_boards(Nobservations_board, + calibration_object_width_n, + calibration_object_height_n) + + mrcal_num_measurements_points(Nobservations_point) + + mrcal_num_measurements_points_triangulated(observations_point_triangulated, + Nobservations_point_triangulated); +} + +int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + return + Ncameras_intrinsics * + num_regularization_terms_percamera(problem_selections, lensmodel) + + + ((problem_selections.do_apply_regularization_unity_cam01 && + problem_selections.do_optimize_extrinsics && + Ncameras_extrinsics > 0) ? 1 : 0); +} + +int mrcal_num_measurements(int Nobservations_board, + int Nobservations_point, + + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + return + mrcal_num_measurements_boards( Nobservations_board, + calibration_object_width_n, + calibration_object_height_n) + + mrcal_num_measurements_points(Nobservations_point) + + mrcal_num_measurements_points_triangulated(observations_point_triangulated, + Nobservations_point_triangulated) + + mrcal_num_measurements_regularization(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel); +} + +static bool has_calobject_warp(mrcal_problem_selections_t problem_selections, + int Nobservations_board) +{ + return problem_selections.do_optimize_calobject_warp && Nobservations_board>0; +} + +int _mrcal_num_j_nonzero(int Nobservations_board, + int Nobservations_point, + + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + // each observation depends on all the parameters for THAT frame and for + // THAT camera. Camera0 doesn't have extrinsics, so I need to loop through + // all my observations + + // Each projected point has an x and y measurement, and each one depends on + // some number of the intrinsic parameters. Parametric models are simple: + // each one depends on ALL of the intrinsics. Splined models are sparse, + // however, and there's only a partial dependence + int Nintrinsics_per_measurement; + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + int run_len = + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1; + Nintrinsics_per_measurement = + (problem_selections.do_optimize_intrinsics_core ? 4 : 0) + + (problem_selections.do_optimize_intrinsics_distortions ? (run_len*run_len) : 0); + } + else + Nintrinsics_per_measurement = + mrcal_num_intrinsics_optimization_params(problem_selections, lensmodel); + + // x depends on fx,cx but NOT on fy, cy. And similarly for y. + if( problem_selections.do_optimize_intrinsics_core && + modelHasCore_fxfycxcy(lensmodel) ) + Nintrinsics_per_measurement -= 2; + + int N = Nobservations_board * ( (problem_selections.do_optimize_frames ? 6 : 0) + + (problem_selections.do_optimize_extrinsics ? 6 : 0) + + (has_calobject_warp(problem_selections,Nobservations_board) ? MRCAL_NSTATE_CALOBJECT_WARP : 0) + + Nintrinsics_per_measurement ); + + // initial estimate counts extrinsics for the reference camera, which need + // to be subtracted off + if(problem_selections.do_optimize_extrinsics) + for(int i=0; i= 0 ) + N += 2*6; + } + + // And the triangulated point observations + if(observations_point_triangulated != NULL && + Nobservations_point_triangulated > 0) + { + // Similar loop as in + // mrcal_num_measurements_points_triangulated_initial_Npoints(). If the + // data layout changes, update this and that + for(int i0=0; i0= 0 ) + Nvars0 += 6; + + int i1 = i0; + + do + { + i1++; + + int Nvars1 = Nintrinsics_per_measurement; + if( problem_selections.do_optimize_extrinsics && + observations_point_triangulated[i1].icam.extrinsics >= 0 ) + Nvars1 += 6; + + N += Nvars0 + Nvars1; + + } while(!observations_point_triangulated[i1].last_in_set); + } + } + + // Regularization + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + if(problem_selections.do_apply_regularization) + { + // Each regularization term depends on + // - two values for distortions + // - one value for the center pixel + N += + Ncameras_intrinsics * + 2 * + num_regularization_terms_percamera(problem_selections, + lensmodel); + // I multiplied by 2, so I double-counted the center pixel + // contributions. Subtract those off + if(problem_selections.do_optimize_intrinsics_core) + N -= Ncameras_intrinsics*2; + } + } + else + N += + Ncameras_intrinsics * + num_regularization_terms_percamera(problem_selections, + lensmodel); + + if(problem_selections.do_apply_regularization_unity_cam01 && + problem_selections.do_optimize_extrinsics && + Ncameras_extrinsics > 0) + { +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: I assume that camera0 is at the reference" +#endif + N += 3; // translation only + } + + return N; +} + +// Used in the spline-based projection function. +// +// See bsplines.py for the derivation of the spline expressions and for +// justification of the 2D scheme +// +// Here we sample two interpolated surfaces at once: one each for the x and y +// focal-length scales +// +// The sampling function assumes evenly spaced knots. +// a,b,c,d are sequential control points +// x is in [0,1] between b and c. Function looks like this: +// double A = fA(x); +// double B = fB(x); +// double C = fC(x); +// double D = fD(x); +// return A*a + B*b + C*c + D*d; +// I need to sample many such 1D segments, so I compute A,B,C,D separately, +// and apply them together +static +void get_sample_coeffs__cubic(double* ABCD, double* ABCDgrad, double x) +{ + double x2 = x*x; + double x3 = x2*x; + ABCD[0] = (-x3 + 3*x2 - 3*x + 1)/6; + ABCD[1] = (3 * x3/2 - 3*x2 + 2)/3; + ABCD[2] = (-3 * x3 + 3*x2 + 3*x + 1)/6; + ABCD[3] = x3 / 6; + + ABCDgrad[0] = -x2/2 + x - 1./2.; + ABCDgrad[1] = 3*x2/2 - 2*x; + ABCDgrad[2] = -3*x2/2 + x + 1./2.; + ABCDgrad[3] = x2 / 2; +} +static +void interp__cubic(double* out, const double* ABCDx, const double* ABCDy, + // control points + const double* c, + int stridey) +{ + double cinterp[4][2]; + const int stridex = 2; + for(int iy=0; iy<4; iy++) + for(int k=0;k<2;k++) + cinterp[iy][k] = + ABCDx[0] * c[iy*stridey + 0*stridex + k] + + ABCDx[1] * c[iy*stridey + 1*stridex + k] + + ABCDx[2] * c[iy*stridey + 2*stridex + k] + + ABCDx[3] * c[iy*stridey + 3*stridex + k]; + for(int k=0;k<2;k++) + out[k] = + ABCDy[0] * cinterp[0][k] + + ABCDy[1] * cinterp[1][k] + + ABCDy[2] * cinterp[2][k] + + ABCDy[3] * cinterp[3][k]; +} +static +void sample_bspline_surface_cubic(double* out, + double* dout_dx, + double* dout_dy, + double* ABCDx_ABCDy, + + double x, double y, + // control points + const double* c, + int stridey + + // stridex is 2: the control points from the + // two surfaces are next to each other. Better + // cache locality maybe + ) +{ + double* ABCDx = &ABCDx_ABCDy[0]; + double* ABCDy = &ABCDx_ABCDy[4]; + + // 4 samples along one dimension, and then one sample along the other + // dimension, using the 4 samples as the control points. Order doesn't + // matter. See bsplines.py + // + // I do this twice: one for each focal length surface + double ABCDgradx[4]; + double ABCDgrady[4]; + get_sample_coeffs__cubic(ABCDx, ABCDgradx, x); + get_sample_coeffs__cubic(ABCDy, ABCDgrady, y); + + // the intrinsics gradient is flatten(ABCDx[0..3] * ABCDy[0..3]) for both x + // and y. By returning ABCD[xy] and not the cartesian products, I make + // smaller temporary data arrays + interp__cubic(out, ABCDx, ABCDy, c, stridey); + interp__cubic(dout_dx, ABCDgradx, ABCDy, c, stridey); + interp__cubic(dout_dy, ABCDx, ABCDgrady, c, stridey); +} +// The sampling function assumes evenly spaced knots. +// a,b,c are sequential control points +// x is in [-1/2,1/2] around b. Function looks like this: +// double A = fA(x); +// double B = fB(x); +// double C = fC(x); +// return A*a + B*b + C*c; +// I need to sample many such 1D segments, so I compute A,B,C separately, +// and apply them together +static +void get_sample_coeffs__quadratic(double* ABC, double* ABCgrad, double x) +{ + double x2 = x*x; + ABC[0] = (4*x2 - 4*x + 1)/8; + ABC[1] = (3 - 4*x2)/4; + ABC[2] = (4*x2 + 4*x + 1)/8; + + ABCgrad[0] = x - 1./2.; + ABCgrad[1] = -2.*x; + ABCgrad[2] = x + 1./2.; +} +static +void interp__quadratic(double* out, const double* ABCx, const double* ABCy, + // control points + const double* c, + int stridey) +{ + double cinterp[3][2]; + const int stridex = 2; + for(int iy=0; iy<3; iy++) + for(int k=0;k<2;k++) + cinterp[iy][k] = + ABCx[0] * c[iy*stridey + 0*stridex + k] + + ABCx[1] * c[iy*stridey + 1*stridex + k] + + ABCx[2] * c[iy*stridey + 2*stridex + k]; + for(int k=0;k<2;k++) + out[k] = + ABCy[0] * cinterp[0][k] + + ABCy[1] * cinterp[1][k] + + ABCy[2] * cinterp[2][k]; +} +static +void sample_bspline_surface_quadratic(double* out, + double* dout_dx, + double* dout_dy, + double* ABCx_ABCy, + + double x, double y, + // control points + const double* c, + int stridey + + // stridex is 2: the control points from the + // two surfaces are next to each other. Better + // cache locality maybe + ) +{ + double* ABCx = &ABCx_ABCy[0]; + double* ABCy = &ABCx_ABCy[3]; + + // 3 samples along one dimension, and then one sample along the other + // dimension, using the 3 samples as the control points. Order doesn't + // matter. See bsplines.py + // + // I do this twice: one for each focal length surface + double ABCgradx[3]; + double ABCgrady[3]; + get_sample_coeffs__quadratic(ABCx, ABCgradx, x); + get_sample_coeffs__quadratic(ABCy, ABCgrady, y); + + // the intrinsics gradient is flatten(ABCx[0..3] * ABCy[0..3]) for both x + // and y. By returning ABC[xy] and not the cartesian products, I make + // smaller temporary data arrays + interp__quadratic(out, ABCx, ABCy, c, stridey); + interp__quadratic(dout_dx, ABCgradx, ABCy, c, stridey); + interp__quadratic(dout_dy, ABCx, ABCgrady, c, stridey); +} + +typedef struct +{ + double _d_rj_rf[3*3]; + double _d_rj_rc[3*3]; + double _d_tj_tf[3*3]; + double _d_tj_rc[3*3]; + + // _d_tj_tc is always identity + // _d_tj_rf is always 0 + // _d_rj_tf is always 0 + // _d_rj_tc is always 0 + +} geometric_gradients_t; + +static +void project_cahvor( // outputs + mrcal_point2_t* restrict q, + mrcal_point2_t* restrict dq_dfxy, + double* restrict dq_dintrinsics_nocore, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* restrict p, + const mrcal_point3_t* restrict dp_drc, + const mrcal_point3_t* restrict dp_dtc, + const mrcal_point3_t* restrict dp_drf, + const mrcal_point3_t* restrict dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + const mrcal_lensmodel_t* lensmodel) +{ + int NdistortionParams = mrcal_lensmodel_num_params(lensmodel) - 4; + + // I perturb p, and then apply the focal length, center pixel stuff + // normally + mrcal_point3_t p_distorted; + + bool need_any_extrinsics_gradients = + ( dq_drcamera != NULL ) || + ( dq_dtcamera != NULL ) || + ( dq_drframe != NULL ) || + ( dq_dtframe != NULL ); + + // distortion parameter layout: + // alpha + // beta + // r0 + // r1 + // r2 + double alpha = intrinsics[4 + 0]; + double beta = intrinsics[4 + 1]; + double r0 = intrinsics[4 + 2]; + double r1 = intrinsics[4 + 3]; + double r2 = intrinsics[4 + 4]; + + double s_al = sin(alpha); + double c_al = cos(alpha); + double s_be = sin(beta); + double c_be = cos(beta); + + // I parametrize the optical axis such that + // - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center + // if both parameters are 0 + // - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both + // NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal + // lock), and that would make my solver unhappy + double o [] = { s_al*c_be, s_be, c_al*c_be }; + double do_dalpha[] = { c_al*c_be, 0, -s_al*c_be }; + double do_dbeta[] = { -s_al*s_be, c_be, -c_al*s_be }; + + double norm2p = norm2_vec(3, p->xyz); + double omega = dot_vec(3, p->xyz, o); + double domega_dalpha = dot_vec(3, p->xyz, do_dalpha); + double domega_dbeta = dot_vec(3, p->xyz, do_dbeta); + + double omega_recip = 1.0 / omega; + double tau = norm2p * omega_recip*omega_recip - 1.0; + double s__dtau_dalphabeta__domega_dalphabeta = -2.0*norm2p * omega_recip*omega_recip*omega_recip; + double dmu_dtau = r1 + 2.0*tau*r2; + double dmu_dxyz[3]; + for(int i=0; i<3; i++) + dmu_dxyz[i] = dmu_dtau * + (2.0 * p->xyz[i] * omega_recip*omega_recip + s__dtau_dalphabeta__domega_dalphabeta * o[i]); + double mu = r0 + tau*r1 + tau*tau*r2; + double s__dmu_dalphabeta__domega_dalphabeta = dmu_dtau * s__dtau_dalphabeta__domega_dalphabeta; + + double dpdistorted_dpcam[3*3] = {}; + std::vector dpdistorted_ddistortion(3*NdistortionParams); + + for(int i=0; i<3; i++) + { + double dmu_ddist[5] = { s__dmu_dalphabeta__domega_dalphabeta * domega_dalpha, + s__dmu_dalphabeta__domega_dalphabeta * domega_dbeta, + 1.0, + tau, + tau * tau }; + + dpdistorted_ddistortion[i*NdistortionParams + 0] = p->xyz[i] * dmu_ddist[0]; + dpdistorted_ddistortion[i*NdistortionParams + 1] = p->xyz[i] * dmu_ddist[1]; + dpdistorted_ddistortion[i*NdistortionParams + 2] = p->xyz[i] * dmu_ddist[2]; + dpdistorted_ddistortion[i*NdistortionParams + 3] = p->xyz[i] * dmu_ddist[3]; + dpdistorted_ddistortion[i*NdistortionParams + 4] = p->xyz[i] * dmu_ddist[4]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= dmu_ddist[0] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= dmu_ddist[1] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 2] -= dmu_ddist[2] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 3] -= dmu_ddist[3] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 4] -= dmu_ddist[4] * omega*o[i]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * domega_dalpha*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * domega_dbeta *o[i]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * omega * do_dalpha[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * omega * do_dbeta [i]; + + dpdistorted_dpcam[3*i + i] = mu+1.0; + for(int j=0; j<3; j++) + { + dpdistorted_dpcam[3*i + j] += (p->xyz[i] - omega*o[i]) * dmu_dxyz[j]; + dpdistorted_dpcam[3*i + j] -= mu*o[i]*o[j]; + } + + p_distorted.xyz[i] = p->xyz[i] + mu * (p->xyz[i] - omega*o[i]); + } + + // q = fxy pxy/pz + cxy + // dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1]) + // dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1]) + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + double pz_recip = 1. / p_distorted.z; + q->x = p_distorted.x*pz_recip * fx + cx; + q->y = p_distorted.y*pz_recip * fy + cy; + + if(need_any_extrinsics_gradients) + { + double dq_dp[2][3] = + { { fx * pz_recip, 0, -fx*p_distorted.x*pz_recip*pz_recip}, + { 0, fy * pz_recip, -fy*p_distorted.y*pz_recip*pz_recip} }; + // This is for the DISTORTED p. + // dq/deee = dq/dpdistorted dpdistorted/dpundistorted dpundistorted/deee + + double dq_dpundistorted[6]; + mul_genN3_gen33_vout(2, (double*)dq_dp, dpdistorted_dpcam, dq_dpundistorted); + + // dq/deee = dq/dp dp/deee + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe); + if( dq_dtframe != NULL ) memcpy(dq_dtframe, dq_dpundistorted, 6*sizeof(double)); + } + else + { + if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drc, (double*)dq_drcamera); + if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtc, (double*)dq_dtcamera); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe ); + if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtf, (double*)dq_dtframe ); + } + } + + if( dq_dintrinsics_nocore != NULL ) + { + for(int i=0; ix = (q->x - cx)/fx; // dqx/dfx + dq_dfxy->y = (q->y - cy)/fy; // dqy/dfy + } +} + +static +bool project_cahvore( // outputs + mrcal_point2_t* restrict q, + mrcal_point2_t* restrict dq_dfxy, + double* restrict dq_dintrinsics_nocore, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* restrict p, + const mrcal_point3_t* restrict dp_drc, + const mrcal_point3_t* restrict dp_dtc, + const mrcal_point3_t* restrict dp_drf, + const mrcal_point3_t* restrict dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + const mrcal_lensmodel_t* lensmodel) +{ + int NdistortionParams = mrcal_lensmodel_num_params(lensmodel) - 4; // This is 8: alpha,beta,R,E + + mrcal_point3_t p_distorted; + double dpdistorted_ddistortion[8*3]; // 8 intrinsics parameters, nvec(p)=3 + double dpdistorted_dpcam[3*3]; + + bool need_any_extrinsics_gradients = + ( dq_drcamera != NULL ) || + ( dq_dtcamera != NULL ) || + ( dq_drframe != NULL ) || + ( dq_dtframe != NULL ); + + if(!project_cahvore_internals( // outputs + &p_distorted, + dq_dintrinsics_nocore ? dpdistorted_ddistortion : NULL, + need_any_extrinsics_gradients ? dpdistorted_dpcam : NULL, + + // inputs + p, + &intrinsics[4], + lensmodel->LENSMODEL_CAHVORE__config.linearity)) + return false; + + ////////////// exactly like in project_cahvor() above. Consolidate. + + // q = fxy pxy/pz + cxy + // dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1]) + // dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1]) + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + double pz_recip = 1. / p_distorted.z; + q->x = p_distorted.x*pz_recip * fx + cx; + q->y = p_distorted.y*pz_recip * fy + cy; + + if(need_any_extrinsics_gradients) + { + double dq_dp[2][3] = + { { fx * pz_recip, 0, -fx*p_distorted.x*pz_recip*pz_recip}, + { 0, fy * pz_recip, -fy*p_distorted.y*pz_recip*pz_recip} }; + // This is for the DISTORTED p. + // dq/deee = dq/dpdistorted dpdistorted/dpundistorted dpundistorted/deee + + double dq_dpundistorted[6]; + mul_genN3_gen33_vout(2, (double*)dq_dp, dpdistorted_dpcam, dq_dpundistorted); + + // dq/deee = dq/dp dp/deee + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe); + if( dq_dtframe != NULL ) memcpy(dq_dtframe, dq_dpundistorted, 6*sizeof(double)); + } + else + { + if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drc, (double*)dq_drcamera); + if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtc, (double*)dq_dtcamera); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe ); + if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtf, (double*)dq_dtframe ); + } + } + + if( dq_dintrinsics_nocore != NULL ) + { + for(int i=0; ix = (q->x - cx)/fx; // dqx/dfx + dq_dfxy->y = (q->y - cy)/fy; // dqy/dfy + } + return true; +} + +// These are all internals for project(). It was getting unwieldy otherwise +static +void _project_point_parametric( // outputs + mrcal_point2_t* q, + mrcal_point2_t* dq_dfxy, double* dq_dintrinsics_nocore, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* p, + const mrcal_point3_t* dp_drc, + const mrcal_point3_t* dp_dtc, + const mrcal_point3_t* dp_drf, + const mrcal_point3_t* dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + const mrcal_lensmodel_t* lensmodel) +{ + // u = distort(p, distortions) + // q = uxy/uz * fxy + cxy + if(!( lensmodel->type == MRCAL_LENSMODEL_PINHOLE || + lensmodel->type == MRCAL_LENSMODEL_STEREOGRAPHIC || + lensmodel->type == MRCAL_LENSMODEL_LONLAT || + lensmodel->type == MRCAL_LENSMODEL_LATLON || + MRCAL_LENSMODEL_IS_OPENCV(lensmodel->type) )) + { + MSG("Unhandled lens model: %d (%s)", + lensmodel->type, mrcal_lensmodel_name_unconfigured(lensmodel)); + assert(0); + } + + mrcal_point3_t dq_dp[2]; + if( lensmodel->type == MRCAL_LENSMODEL_PINHOLE ) + mrcal_project_pinhole(q, dq_dp, + p, 1, intrinsics); + else if(lensmodel->type == MRCAL_LENSMODEL_STEREOGRAPHIC) + mrcal_project_stereographic(q, dq_dp, + p, 1, intrinsics); + else if(lensmodel->type == MRCAL_LENSMODEL_LONLAT) + mrcal_project_lonlat(q, dq_dp, + p, 1, intrinsics); + else if(lensmodel->type == MRCAL_LENSMODEL_LATLON) + mrcal_project_latlon(q, dq_dp, + p, 1, intrinsics); + else + { + int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + _mrcal_project_internal_opencv( q, dq_dp, + dq_dintrinsics_nocore, + p, 1, intrinsics, Nintrinsics); + } + + // dq/deee = dq/dp dp/deee + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drf, (double*)dq_drframe); + if( dq_dtframe != NULL ) memcpy(dq_dtframe, (double*)dq_dp, 6*sizeof(double)); + } + else + { + if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drc, (double*)dq_drcamera); + if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_dtc, (double*)dq_dtcamera); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drf, (double*)dq_drframe ); + if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_dtf, (double*)dq_dtframe ); + } + + // I have the projection, and I now need to propagate the gradients + if( dq_dfxy ) + { + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + + // I have the projection, and I now need to propagate the gradients + // xy = fxy * distort(xy)/distort(z) + cxy + dq_dfxy->x = (q->x - cx)/fx; // dqx/dfx + dq_dfxy->y = (q->y - cy)/fy; // dqy/dfy + } +} + +// Compute a pinhole projection using a constant fxy, cxy +void mrcal_project_pinhole( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dv, // May be NULL. Each point + // gets a block of 2 mrcal_point3_t + // objects + + // input + const mrcal_point3_t* v, + int N, + const double* fxycxy) +{ + const double fx = fxycxy[0]; + const double fy = fxycxy[1]; + const double cx = fxycxy[2]; + const double cy = fxycxy[3]; + + // q = fxy pxy/pz + cxy + // dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1]) + // dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1]) + for(int i=0; ix = v[i].x*pz_recip * fx + cx; + q->y = v[i].y*pz_recip * fy + cy; + + if(dq_dv) + { + dq_dv[2*i + 0].x = fx * pz_recip; + dq_dv[2*i + 0].y = 0; + dq_dv[2*i + 0].z = -fx*v[i].x*pz_recip*pz_recip; + + dq_dv[2*i + 1].x = 0; + dq_dv[2*i + 1].y = fy * pz_recip; + dq_dv[2*i + 1].z = -fy*v[i].y*pz_recip*pz_recip; + } + } +} + +// Compute a pinhole unprojection using a constant fxy, cxy +void mrcal_unproject_pinhole( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, // May be NULL. Each point + // gets a block of 3 + // mrcal_point2_t objects + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy) +{ + const double fx = fxycxy[0]; + const double fy = fxycxy[1]; + const double cx = fxycxy[2]; + const double cy = fxycxy[3]; + + double fx_recip = 1./fx; + double fy_recip = 1./fy; + for(int i=0; i u = (q-c)/f + // mag(u) = tan(th/2)*2 + // + // So I can compute th. az comes from the direction of u. This is enough to + // compute everything. th is in [0,pi]. + // + // [ sin(th) cos(az) ] [ cos(az) ] + // v = [ sin(th) sin(az) ] ~ [ sin(az) ] + // [ cos(th) ] [ 1/tan(th) ] + // + // mag(u) = tan(th/2)*2 -> mag(u)/2 = tan(th/2) -> + // tan(th) = mag(u) / (1 - (mag(u)/2)^2) + // 1/tan(th) = (1 - 1/4*mag(u)^2) / mag(u) + // + // This has a singularity at u=0 (imager center). But I can scale v to avoid + // this. So + // + // [ cos(az) mag(u) ] + // v = [ sin(az) mag(u) ] + // [ 1 - 1/4*mag(u)^2 ] + // + // I can simplify this even more. az = atan2(u.y,u.x). cos(az) = u.x/mag(u) -> + // + // [ u.x ] + // v = [ u.y ] + // [ 1 - 1/4 mag(u)^2 ] + // + // Test script to confirm that the project/unproject expressions are + // correct. unproj(proj(v))/v should be a constant + // + // import numpy as np + // import numpysane as nps + // f = 2000 + // c = 1000 + // def proj(v): + // m = nps.mag(v) + // scale = 2.0 / (m + v[..., 2]) + // u = v[..., :2] * nps.dummy(scale, -1) + // return u * f + c + // def unproj(q): + // u = (q-c)/f + // muxy = nps.mag(u[..., :2]) + // m = nps.mag(u) + // return nps.mv(nps.cat( u[..., 0], + // u[..., 1], + // 1 - 1./4.* m*m), + // 0, -1) + // v = np.array(((1., 2., 3.), + // (3., -2., -4.))) + // print( unproj(proj(v)) / v) + double fx_recip = 1./fx; + double fy_recip = 1./fy; + for(int i=0; i + // dqx/dv = 1/(1 + (vx/vz)^2) 1/vz^2 ( dvx/dv vz - vx dvz/dv ) = + // = [vz 0 -vx] / (vx^2 + vz^2) + // + // qy ~ arcsin( vy / mag(v) ) -> + // dqy/dv = 1 / sqrt( 1 - vy^2/norm2(v) ) 1/norm2(v) (dvy/dv mag(v) - dmag(v)/dv vy) + // = 1 / sqrt( norm2(v) - vy^2 ) 1/mag(v) ( [0 mag(v) 0] - v/mag(v) vy) + // = 1 / sqrt( norm2(v) - vy^2 ) ( [0 1 0] - v/norm2(v) vy) + // = 1 / sqrt( vx^2 + vz^2 ) ( [0 1 0] - v/norm2(v) vy) + for(int i=0; i vx = vz tan(lon) + // -> 1-sin^2(lat) = vz^2 (1 + tan^2(lon)) = + // cos^2(lat) = (vz/cos(lon))^2 + // + // -> vx = cos(lat) sin(lon) + // vy = sin(lat) + // vz = cos(lat) cos(lon) + // + // mag(v) is arbitrary, and I can simplify: + // + // -> v_unnormalized_x = sin(lon) + // v_unnormalized_y = tan(lat) + // v_unnormalized_z = cos(lon) + // + // If the computational cost of tan(lat) is smaller than of + // sin(lat),cos(lat) and 2 multiplications, then this is a better + // representation. A quick web search tells me that the cost of sincos ~ the + // cost of either sin or cos. And that tan is more costly. So I use the + // normalized form + // + // dv/dlon = [ cos(lat) cos(lon) 0 -cos(lat) sin(lon)] + // dv/dlat = [-sin(lat) sin(lon) cos(lat) -sin(lat) cos(lon)] + double fx_recip = 1./fx; + double fy_recip = 1./fy; + for(int i=0; i u_width_needed/(Nknots-1) = u_interval_size * (1 - Nknots_margin/(Nknots - 1)) + // ---> u_width_needed = u_interval_size * (Nknots - 1 - Nknots_margin) + // ---> u_interval_size = u_width_needed / (Nknots - 1 - Nknots_margin) + int Nknots_margin; + if(config->order == 2) + { + Nknots_margin = 1; + if(config->Nx < 3 || config->Ny < 3) + { + MSG("Quadratic splines: absolute minimum Nx, Ny is 3. Got Nx=%d Ny=%d. Barfing out", + config->Nx, config->Ny); + assert(0); + } + } + else if(config->order == 3) + { + Nknots_margin = 2; + if(config->Nx < 4 || config->Ny < 4) + { + MSG("Cubic splines: absolute minimum Nx, Ny is 4. Got Nx=%d Ny=%d. Barfing out", + config->Nx, config->Ny); + assert(0); + } + } + else + { + MSG("I only support spline order 2 and 3"); + assert(0); + } + + double th_edge_x = (double)config->fov_x_deg/2. * M_PI / 180.; + double u_edge_x = tan(th_edge_x / 2.) * 2; + precomputed->segments_per_u = (config->Nx - 1 - Nknots_margin) / (u_edge_x*2.); +} + +// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper +// only +void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed, + const mrcal_lensmodel_t* lensmodel) +{ + // currently only this model has anything + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + _mrcal_precompute_lensmodel_data_MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC + ( &precomputed->LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed, + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config ); + precomputed->ready = true; +} + +bool mrcal_knots_for_splined_models( // buffers must hold at least + // config->Nx and config->Ny values + // respectively + double* ux, double* uy, + const mrcal_lensmodel_t* lensmodel) +{ + if(lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + MSG("This function works only with the MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC model. '%s' passed in", + mrcal_lensmodel_name_unconfigured(lensmodel)); + return false; + } + + mrcal_projection_precomputed_t precomputed_all; + _mrcal_precompute_lensmodel_data(&precomputed_all, lensmodel); + + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config; + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t* precomputed = + &precomputed_all.LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed; + + // The logic I'm reversing is + // double ix = u.x*segments_per_u + (double)(Nx-1)/2.; + for(int i=0; iNx; i++) + ux[i] = + ((double)i - (double)(config->Nx-1)/2.) / + precomputed->segments_per_u; + for(int i=0; iNy; i++) + uy[i] = + ((double)i - (double)(config->Ny-1)/2.) / + precomputed->segments_per_u; + return true; +} + +static int get_Ngradients(const mrcal_lensmodel_t* lensmodel, + int Nintrinsics) +{ + int N = 0; + bool has_core = modelHasCore_fxfycxcy(lensmodel); + bool has_dense_intrinsics_grad = (lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + bool has_sparse_intrinsics_grad = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + int runlen = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) ? + (lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1) : + 0; + if(has_core) + // qx(fx) and qy(fy) + N += 2; + if(has_dense_intrinsics_grad) + // each of (qx,qy) depends on all the non-core intrinsics + N += 2 * (Nintrinsics-4); + if(has_sparse_intrinsics_grad) + { + // spline coefficients + N += 2*runlen; + } + + return N; +} + +static +void propagate_extrinsics__splined( // output + mrcal_point3_t* dq_deee, + // input + const mrcal_point3_t* dp_deee, + const double* duxy_dp, + const double* ddeltau_dux, + const double* ddeltau_duy, + const double fx, + const double fy) +{ + mrcal_point3_t du_deee[2]; + mul_genN3_gen33_vout(2, (double*)duxy_dp, (double*)dp_deee, (double*)du_deee); + + for(int i=0; i<3; i++) + { + dq_deee[0].xyz[i] = + fx * + ( du_deee[0].xyz[i] * (1. + ddeltau_dux[0]) + + ddeltau_duy[0] * du_deee[1].xyz[i]); + dq_deee[1].xyz[i] = + fy * + ( du_deee[1].xyz[i] * (1. + ddeltau_duy[1]) + + ddeltau_dux[1] * du_deee[0].xyz[i]); + } +} +static +void propagate_extrinsics_cam0__splined( // output + mrcal_point3_t* dq_deee, + // input + const double* dux_dp, + const double* duy_dp, + const double* ddeltau_dux, + const double* ddeltau_duy, + const double fx, + const double fy) +{ + for(int i=0; i<3; i++) + { + dq_deee[0].xyz[i] = + fx * + ( dux_dp[i] * (1. + ddeltau_dux[0]) + + ddeltau_duy[0] * duy_dp[i]); + dq_deee[1].xyz[i] = + fy * + ( duy_dp[i] * (1. + ddeltau_duy[1]) + + ddeltau_dux[1] * dux_dp[i]); + } +} +static +void _project_point_splined( // outputs + mrcal_point2_t* q, + mrcal_point2_t* dq_dfxy, + + double* grad_ABCDx_ABCDy, + int* ivar0, + + // Gradient outputs. May be NULL + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* restrict p, + const mrcal_point3_t* restrict dp_drc, + const mrcal_point3_t* restrict dp_dtc, + const mrcal_point3_t* restrict dp_drf, + const mrcal_point3_t* restrict dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + int spline_order, + uint16_t Nx, uint16_t Ny, + double segments_per_u) +{ + // projections out-of-bounds will yield SOME value (function remains + // continuous as we move out-of-bounds), but it wont be particularly + // meaningful + + + // stereographic projection: + // (from https://en.wikipedia.org/wiki/Fisheye_lens) + // u = xy_unit * tan(th/2) * 2 + // + // I compute the normalized (focal-length = 1) projection, and + // use that to look-up deltau + + // th is the angle between the observation and the projection + // center + // + // sin(th) = mag_pxy/mag_p + // cos(th) = z/mag_p + // tan(th/2) = sin(th) / (1 + cos(th)) + + // tan(th/2) = mag_pxy/mag_p / (1 + z/mag_p) = + // = mag_pxy / (mag_p + z) + // u = xy_unit * tan(th/2) * 2 = + // = xy/mag_pxy * mag_pxy/(mag_p + z) * 2 = + // = xy / (mag_p + z) * 2 + // + // The stereographic projection is used to query the spline surface, and it + // is also the projection baseline. I do + // + // q = (u + deltau(u)) * f + c + // + // If the spline surface is at 0 (deltau == 0) then this is a pure + // stereographic projection + double mag_p = sqrt( p->x*p->x + + p->y*p->y + + p->z*p->z ); + double scale = 2.0 / (mag_p + p->z); + + mrcal_point2_t u = {.x = p->x * scale, + .y = p->y * scale}; + // du/dp = d/dp ( xy * scale ) + // = pxy dscale/dp + [I; 0] scale + // dscale/dp = d (2.0 / (mag_p + p->z))/dp = + // = -2/()^2 ( [0,0,1] + dmag/dp) + // = -2/()^2 ( [0,0,1] + 2pt/2mag) + // = -2 scale^2/4 ( [0,0,1] + pt/mag) + // = -scale^2/2 * ( [0,0,1] + pt/mag ) + // = A*[0,0,1] + B*pt + double A = -scale*scale / 2.; + double B = A / mag_p; + double du_dp[2][3] = { { p->x * (B * p->x) + scale, + p->x * (B * p->y), + p->x * (B * p->z + A) }, + { p->y * (B * p->x), + p->y * (B * p->y) + scale, + p->y * (B * p->z + A) } }; + + double ix = u.x*segments_per_u + (double)(Nx-1)/2.; + double iy = u.y*segments_per_u + (double)(Ny-1)/2.; + + mrcal_point2_t deltau; + double ddeltau_dux[2]; + double ddeltau_duy[2]; + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + + if( spline_order == 3 ) + { + int ix0 = (int)ix; + int iy0 = (int)iy; + + // If out-of-bounds, clamp to the nearest valid spline segment. The + // projection will fly off to infinity quickly (we're extrapolating a + // polynomial), but at least it'll stay continuous + if( ix0 < 1) ix0 = 1; + else if(ix0 > Nx-3) ix0 = Nx-3; + if( iy0 < 1) iy0 = 1; + else if(iy0 > Ny-3) iy0 = Ny-3; + + *ivar0 = + 4 + // skip the core + 2*( (iy0-1)*Nx + + (ix0-1) ); + + sample_bspline_surface_cubic(deltau.xy, + ddeltau_dux, ddeltau_duy, + grad_ABCDx_ABCDy, + ix - ix0, iy - iy0, + + // control points + &intrinsics[*ivar0], + 2*Nx); + } + else if( spline_order == 2 ) + { + int ix0 = (int)(ix + 0.5); + int iy0 = (int)(iy + 0.5); + + // If out-of-bounds, clamp to the nearest valid spline segment. The + // projection will fly off to infinity quickly (we're extrapolating a + // polynomial), but at least it'll stay continuous + if( ix0 < 1) ix0 = 1; + else if(ix0 > Nx-2) ix0 = Nx-2; + if( iy0 < 1) iy0 = 1; + else if(iy0 > Ny-2) iy0 = Ny-2; + + *ivar0 = + 4 + // skip the core + 2*( (iy0-1)*Nx + + (ix0-1) ); + + sample_bspline_surface_quadratic(deltau.xy, + ddeltau_dux, ddeltau_duy, + grad_ABCDx_ABCDy, + ix - ix0, iy - iy0, + + // control points + &intrinsics[*ivar0], + 2*Nx); + } + else + { + MSG("I only support spline order==2 or 3. Somehow got %d. This is a bug. Barfing", + spline_order); + assert(0); + } + + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Extrinsics: + // dqx/deee = fx (dux/deee + ddeltaux/du du/deee) + // = fx ( dux/deee (1 + ddeltaux/dux) + ddeltaux/duy duy/deee) + // dqy/deee = fy ( duy/deee (1 + ddeltauy/duy) + ddeltauy/dux dux/deee) + q->x = (u.x + deltau.x) * fx + cx; + q->y = (u.y + deltau.y) * fy + cy; + + if( dq_dfxy ) + { + // I have the projection, and I now need to propagate the gradients + // xy = fxy * distort(xy)/distort(z) + cxy + dq_dfxy->x = u.x + deltau.x; + dq_dfxy->y = u.y + deltau.y; + } + + // convert ddeltau_dixy to ddeltau_duxy + for(int i=0; i<2; i++) + { + ddeltau_dux[i] *= segments_per_u; + ddeltau_duy[i] *= segments_per_u; + } + + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera->xyz, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera->xyz, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) + propagate_extrinsics__splined( dq_drframe, dp_drf, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_dtframe != NULL ) + propagate_extrinsics_cam0__splined( dq_dtframe, + du_dp[0], du_dp[1], + ddeltau_dux, ddeltau_duy, + fx, fy); + } + else + { + if( dq_drcamera != NULL ) + propagate_extrinsics__splined( dq_drcamera, dp_drc, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_dtcamera != NULL ) + propagate_extrinsics__splined( dq_dtcamera, dp_dtc, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_drframe != NULL ) + propagate_extrinsics__splined( dq_drframe, dp_drf, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_dtframe != NULL ) + propagate_extrinsics__splined( dq_dtframe, dp_dtf, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + } +} + +typedef struct +{ + double* pool; + uint16_t run_side_length; + uint16_t ivar_stridey; +} gradient_sparse_meta_t; + + +// This is internal to project() +static +void _propagate_extrinsics_one(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* drj_dparam, + const double* dtj_dparam, + const double* d_Rj_rj) +{ + // dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + for(int i=0; i<3; i++) + { + mul_vec3_gen33_vout( pt_ref->xyz, &d_Rj_rj[9*i], dp_dparam[i].xyz ); + mul_vec3_gen33 ( dp_dparam[i].xyz, drj_dparam); + add_vec(3, dp_dparam[i].xyz, &dtj_dparam[3*i] ); + } +} +static +void _propagate_extrinsics_one_rzero(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* dtj_dparam, + const double* d_Rj_rj) +{ + // dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + memcpy(dp_dparam->xyz, dtj_dparam, 9*sizeof(double)); +} +static +void _propagate_extrinsics_one_tzero(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* drj_dparam, + const double* d_Rj_rj) +{ + // dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + for(int i=0; i<3; i++) + { + mul_vec3_gen33_vout( pt_ref->xyz, &d_Rj_rj[9*i], dp_dparam[i].xyz ); + mul_vec3_gen33 ( dp_dparam[i].xyz, drj_dparam); + } +} +static +void _propagate_extrinsics_one_rzero_tidentity(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* d_Rj_rj) +{ + dp_dparam[0] = (mrcal_point3_t){.x = 1.0}; + dp_dparam[1] = (mrcal_point3_t){.y = 1.0}; + dp_dparam[2] = (mrcal_point3_t){.z = 1.0}; +} + +static +void _propagate_extrinsics_one_cam0(mrcal_point3_t* dp_rf, + const mrcal_point3_t* pt_ref, + const double* _d_Rf_rf) +{ + // dRj[row0]/drj is 3x3 matrix at &_d_Rf_rf[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + for(int i=0; i<3; i++) + mul_vec3_gen33_vout( pt_ref->xyz, &_d_Rf_rf[9*i], dp_rf[i].xyz ); +} +static +mrcal_point3_t _propagate_extrinsics( // output + mrcal_point3_t* _dp_drc, + mrcal_point3_t* _dp_dtc, + mrcal_point3_t* _dp_drf, + mrcal_point3_t* _dp_dtf, + mrcal_point3_t** dp_drc, + mrcal_point3_t** dp_dtc, + mrcal_point3_t** dp_drf, + mrcal_point3_t** dp_dtf, + + // input + const mrcal_point3_t* pt_ref, + const geometric_gradients_t* gg, + const double* Rj, const double* d_Rj_rj, + const double* _tj ) +{ + // Rj * pt + tj -> pt + mrcal_point3_t p; + mul_vec3_gen33t_vout(pt_ref->xyz, Rj, p.xyz); + add_vec(3, p.xyz, _tj); + + if(gg != NULL) + { + _propagate_extrinsics_one( _dp_drc, pt_ref, gg->_d_rj_rc, gg->_d_tj_rc, d_Rj_rj); + _propagate_extrinsics_one_rzero_tidentity(_dp_dtc, pt_ref, d_Rj_rj); + _propagate_extrinsics_one_tzero( _dp_drf, pt_ref, gg->_d_rj_rf, d_Rj_rj); + _propagate_extrinsics_one_rzero( _dp_dtf, pt_ref, gg->_d_tj_tf, d_Rj_rj); + *dp_drc = _dp_drc; + *dp_dtc = _dp_dtc; + *dp_drf = _dp_drf; + *dp_dtf = _dp_dtf; + } + else + { + // camera is at the reference. The "joint" coord system is the "frame" + // coord system + // + // p_cam = Rf p_ref + tf + // + // dp/drc = 0 + // dp/dtc = 0 + // dp/drf = reshape(dRf_drf p_ref) + // dp/dtf = I + _propagate_extrinsics_one_cam0(_dp_drf, pt_ref, d_Rj_rj); + + *dp_drc = NULL; + *dp_dtc = NULL; + *dp_drf = _dp_drf; + *dp_dtf = NULL; // this is I. The user of this MUST know to interpret + // it that way + } + return p; +} +// This is internal to project() +static +void _project_point( // outputs + mrcal_point2_t* q, + mrcal_point2_t* p_dq_dfxy, + double* p_dq_dintrinsics_nocore, + double* gradient_sparse_meta_pool, + int runlen, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + mrcal_calobject_warp_t* restrict dq_dcalobject_warp, + int* restrict * dq_dintrinsics_pool_int, + // inputs + const mrcal_point3_t* p, + const double* restrict intrinsics, + const mrcal_lensmodel_t* lensmodel, + const mrcal_calobject_warp_t* dpt_refz_dwarp, + // if NULL then the camera is at the reference + bool camera_at_identity, + const double* Rj, + const int Nintrinsics, + const mrcal_projection_precomputed_t* precomputed, + const mrcal_point3_t* dp_drc, + const mrcal_point3_t* dp_dtc, + const mrcal_point3_t* dp_drf, + const mrcal_point3_t* dp_dtf) +{ + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + // only need 3+3 for quadratic splines + double grad_ABCDx_ABCDy[4+4]; + int ivar0; + + _project_point_splined( // outputs + q, p_dq_dfxy, + grad_ABCDx_ABCDy, + &ivar0, + + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order, + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx, + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny, + precomputed->LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed.segments_per_u); + // WARNING: if I could assume that dq_dintrinsics_pool_double!=NULL then I wouldnt need to copy the context + if(*dq_dintrinsics_pool_int != NULL) + { + *((*dq_dintrinsics_pool_int)++) = ivar0; + memcpy(gradient_sparse_meta_pool, + grad_ABCDx_ABCDy, + sizeof(double)*runlen*2); + } + } + else if(lensmodel->type == MRCAL_LENSMODEL_CAHVOR) + { + project_cahvor( // outputs + q,p_dq_dfxy, + p_dq_dintrinsics_nocore, + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel); + } + else if(lensmodel->type == MRCAL_LENSMODEL_CAHVORE) + { + if(!project_cahvore( // outputs + q,p_dq_dfxy, + p_dq_dintrinsics_nocore, + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel)) + { + MSG("CAHVORE PROJECTION OF (%f,%f,%f) FAILED. I don't know what to do. Setting result and all gradients to 0", + p->x, p->y, p->z); + memset(q, 0, sizeof(*q)); + if(p_dq_dfxy) memset(p_dq_dfxy, 0, sizeof(*p_dq_dfxy)); + if(p_dq_dintrinsics_nocore) memset(p_dq_dintrinsics_nocore, 0, sizeof(*p_dq_dintrinsics_nocore) * 2 * (Nintrinsics-4)); + if(dq_drcamera) memset(dq_drcamera, 0, sizeof(*dq_drcamera)); + if(dq_dtcamera) memset(dq_dtcamera, 0, sizeof(*dq_dtcamera)); + if(dq_drframe) memset(dq_drframe, 0, sizeof(*dq_drframe)); + if(dq_dtframe) memset(dq_dtframe, 0, sizeof(*dq_dtframe)); + + } + } + else + { + _project_point_parametric( // outputs + q,p_dq_dfxy, + p_dq_dintrinsics_nocore, + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel); + } + + if( dq_dcalobject_warp != NULL && dpt_refz_dwarp != NULL ) + { + // p = proj(Rc Rf warp(x) + Rc tf + tc); + // dp/dw = dp/dRcRf(warp(x)) dR(warp(x))/dwarp(x) dwarp/dw = + // = dp/dtc RcRf dwarp/dw + // dp/dtc is dq_dtcamera + // R is rodrigues(rj) + // dwarp/dw = [0 0 0 ...] + // [0 0 0 ...] + // [a b c ...] + // Let R = [r0 r1 r2] + // dp/dw = dp/dt [a r2 b r2] = + // [a dp/dt r2 b dp/dt r2 ...] + mrcal_point3_t* p_dq_dt; + if(!camera_at_identity) p_dq_dt = dq_dtcamera; + else p_dq_dt = dq_dtframe; + double d[] = + { p_dq_dt[0].xyz[0] * Rj[0*3 + 2] + + p_dq_dt[0].xyz[1] * Rj[1*3 + 2] + + p_dq_dt[0].xyz[2] * Rj[2*3 + 2], + p_dq_dt[1].xyz[0] * Rj[0*3 + 2] + + p_dq_dt[1].xyz[1] * Rj[1*3 + 2] + + p_dq_dt[1].xyz[2] * Rj[2*3 + 2]}; + + for(int i=0; ivalues[i]; + dq_dcalobject_warp[1].values[i] = d[1]*dpt_refz_dwarp->values[i]; + } + } +} + +// Projects 3D point(s), and reports the projection, and all the gradients. This +// is the main internal callback in the optimizer. This operates in one of two modes: +// +// if(calibration_object_width_n == 0) then we're projecting ONE point. In world +// coords this point is at frame_rt->t. frame_rt->r is not referenced. q and the +// gradients reference 2 values (x,y in the imager) +// +// if(calibration_object_width_n > 0) then we're projecting a whole calibration +// object. The pose of this object is given in frame_rt. We project ALL +// calibration_object_width_n*calibration_object_height_n points. q and the +// gradients reference ALL of these points +static +void project( // out + mrcal_point2_t* restrict q, + + // The intrinsics gradients. These are split among several arrays. + // High-parameter-count lens models can return their gradients + // sparsely. All the actual gradient values live in + // dq_dintrinsics_pool_double, a buffer supplied by the caller. If + // dq_dintrinsics_pool_double is not NULL, the rest of the + // variables are assumed non-NULL, and we compute all the + // intrinsics gradients. Conversely, if dq_dintrinsics_pool_double + // is NULL, no intrinsics gradients are computed + double* restrict dq_dintrinsics_pool_double, + int* restrict dq_dintrinsics_pool_int, + double** restrict dq_dfxy, + double** restrict dq_dintrinsics_nocore, + gradient_sparse_meta_t* gradient_sparse_meta, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + mrcal_calobject_warp_t* restrict dq_dcalobject_warp, + + // in + + // everything; includes the core, if there is one + const double* restrict intrinsics, + const mrcal_pose_t* restrict camera_rt, + const mrcal_pose_t* restrict frame_rt, + const mrcal_calobject_warp_t* restrict calobject_warp, + + bool camera_at_identity, // if true, camera_rt is unused + const mrcal_lensmodel_t* lensmodel, + const mrcal_projection_precomputed_t* precomputed, + + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n) +{ + assert(precomputed->ready); + + // Parametric and non-parametric models do different things: + // + // parametric models: + // u = distort(p, distortions) + // q = uxy/uz * fxy + cxy + // + // extrinsic gradients: + // dqx/deee = d( ux/uz * fx + cx)/deee = + // = fx d(ux/uz)/deee = + // = fx/uz^2 ( uz dux/deee - duz/deee ux ) + // + // nonparametric (splined) models + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Extrinsics: + // dqx/deee = fx (dux/deee + ddeltaux/du du/deee) + // = fx ( dux/deee (1 + ddeltaux/dux) + ddeltaux/duy duy/deee) + // dqy/deee = fy ( duy/deee (1 + ddeltauy/duy) + ddeltauy/dux dux/deee) + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // So the two kinds of models have completely different expressions for + // their gradients, and I implement them separately + + const int Npoints = + calibration_object_width_n ? + calibration_object_width_n*calibration_object_height_n : 1; + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + + // I need to compose two transformations + // + // (object in reference frame) -> [frame transform] -> (object in the reference frame) -> + // -> [camera rt] -> (camera frame) + // + // Note that here the frame transform transforms TO the reference frame and + // the camera transform transforms FROM the reference frame. This is how my + // data is expected to be set up + // + // [Rc tc] [Rf tf] = [Rc*Rf Rc*tf + tc] + // [0 1 ] [0 1 ] [0 1 ] + // + // This transformation (and its gradients) is handled by mrcal_compose_rt() + // I refer to the camera*frame transform as the "joint" transform, or the + // letter j + geometric_gradients_t gg; + + double _joint_rt[6]; + double* joint_rt; + + // The caller has an odd-looking array reference [-3]. This is intended, but + // the compiler throws a warning. I silence it here. gcc-10 produces a very + // strange-looking warning that was reported to the maintainers: + // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=97261 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" + mrcal_pose_t frame_rt_validr = {.t = frame_rt->t}; +#pragma GCC diagnostic pop + if(calibration_object_width_n) frame_rt_validr.r = frame_rt->r; + + if(!camera_at_identity) + { + // make sure I can pass mrcal_pose_t.r as an rt[] transformation + mrcal_compose_rt( _joint_rt, + gg._d_rj_rc, gg._d_rj_rf, + gg._d_tj_rc, gg._d_tj_tf, + camera_rt ->r.xyz, + frame_rt_validr.r.xyz); + joint_rt = _joint_rt; + } + else + { + // We're looking at the reference frame, so this camera transform is + // fixed at the identity. We don't need to compose anything, nor + // propagate gradients for the camera extrinsics, since those don't + // exist in the parameter vector + + // Here the "joint" transform is just the "frame" transform + joint_rt = frame_rt_validr.r.xyz; + } + + // Not using OpenCV distortions, the distortion and projection are not + // coupled + double Rj[3*3]; + double d_Rj_rj[9*3]; + + mrcal_R_from_r(Rj, d_Rj_rj, joint_rt); + + mrcal_point2_t* p_dq_dfxy = NULL; + double* p_dq_dintrinsics_nocore = NULL; + bool has_core = modelHasCore_fxfycxcy(lensmodel); + bool has_dense_intrinsics_grad = (lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + bool has_sparse_intrinsics_grad = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + int runlen = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) ? + (lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1) : + 0; + + if(dq_dintrinsics_pool_double != NULL) + { + // nothing by default + *dq_dfxy = NULL; + *dq_dintrinsics_nocore = NULL; + gradient_sparse_meta->pool = NULL; + int ivar_pool = 0; + + if(has_core) + { + // Each point produces 2 measurements. Each one depends on ONE fxy + // element. So Npoints*2 of these + *dq_dfxy = &dq_dintrinsics_pool_double[ivar_pool]; + p_dq_dfxy = (mrcal_point2_t*)*dq_dfxy; + ivar_pool += Npoints*2; + } + if(has_dense_intrinsics_grad) + { + *dq_dintrinsics_nocore = p_dq_dintrinsics_nocore = &dq_dintrinsics_pool_double[ivar_pool]; + ivar_pool += Npoints*2 * (Nintrinsics-4); + } + if(has_sparse_intrinsics_grad) + { + if(lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + MSG("Unhandled lens model: %d (%s)", + lensmodel->type, + mrcal_lensmodel_name_unconfigured(lensmodel)); + assert(0); + } + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config; + *gradient_sparse_meta = + { + &dq_dintrinsics_pool_double[ivar_pool], + static_cast(config->order+1), + static_cast(2*config->Nx), + }; + ivar_pool += Npoints*2 * runlen; + } + } + + // These are produced by _propagate_extrinsics() and consumed by + // _project_point() + mrcal_point3_t _dp_drc[3]; + mrcal_point3_t _dp_dtc[3]; + mrcal_point3_t _dp_drf[3]; + mrcal_point3_t _dp_dtf[3]; + mrcal_point3_t* dp_drc; + mrcal_point3_t* dp_dtc; + mrcal_point3_t* dp_drf; + mrcal_point3_t* dp_dtf; + + if( calibration_object_width_n == 0 ) + { // projecting discrete points + mrcal_point3_t pt_ref = {}; // Unused + mrcal_point3_t p = + _propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf, + &dp_drc,&dp_dtc,&dp_drf,&dp_dtf, + &pt_ref, + camera_at_identity ? NULL : &gg, + Rj, d_Rj_rj, &joint_rt[3]); + _project_point( q, + p_dq_dfxy, p_dq_dintrinsics_nocore, + gradient_sparse_meta ? gradient_sparse_meta->pool : NULL, + runlen, + dq_drcamera, dq_dtcamera, dq_drframe, dq_dtframe, NULL, + &dq_dintrinsics_pool_int, + + &p, + intrinsics, lensmodel, + NULL, + camera_at_identity, Rj, + Nintrinsics, + precomputed, + dp_drc,dp_dtc,dp_drf,dp_dtf); + } + else + { // projecting a chessboard + int i_pt = 0; + // The calibration object has a simple grid geometry + for(int y = 0; yx2 * dx; + pt_ref.z += calobject_warp->y2 * dy; + dpt_refz_dwarp.x2 = dx; + dpt_refz_dwarp.y2 = dy; + } + + mrcal_point3_t p = + _propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf, + &dp_drc,&dp_dtc,&dp_drf,&dp_dtf, + &pt_ref, + camera_at_identity ? NULL : &gg, + Rj, d_Rj_rj, &joint_rt[3]); + + mrcal_point3_t* dq_drcamera_here = dq_drcamera ? &dq_drcamera [i_pt*2] : NULL; + mrcal_point3_t* dq_dtcamera_here = dq_dtcamera ? &dq_dtcamera [i_pt*2] : NULL; + mrcal_point3_t* dq_drframe_here = dq_drframe ? &dq_drframe [i_pt*2] : NULL; + mrcal_point3_t* dq_dtframe_here = dq_dtframe ? &dq_dtframe [i_pt*2] : NULL; + mrcal_calobject_warp_t* dq_dcalobject_warp_here = dq_dcalobject_warp ? &dq_dcalobject_warp [i_pt*2] : NULL; + + mrcal_point3_t dq_dtcamera_here_dummy[2]; + mrcal_point3_t dq_dtframe_here_dummy [2]; + if(dq_dcalobject_warp) + { + // I need all translation gradients to be available to + // compute the calobject_warp gradients (see the end of the + // _project_point() function above). So I compute those even + // if the caller didn't ask for them + if(!dq_dtcamera_here) dq_dtcamera_here = dq_dtcamera_here_dummy; + if(!dq_dtframe_here) dq_dtframe_here = dq_dtframe_here_dummy; + } + + _project_point(&q[i_pt], + p_dq_dfxy ? &p_dq_dfxy[i_pt] : NULL, + p_dq_dintrinsics_nocore ? &p_dq_dintrinsics_nocore[2*(Nintrinsics-4)*i_pt] : NULL, + gradient_sparse_meta ? &gradient_sparse_meta->pool[i_pt*runlen*2] : NULL, + runlen, + dq_drcamera_here, dq_dtcamera_here, dq_drframe_here, dq_dtframe_here, dq_dcalobject_warp_here, + &dq_dintrinsics_pool_int, + &p, + intrinsics, lensmodel, + &dpt_refz_dwarp, + camera_at_identity, Rj, + Nintrinsics, + precomputed, + dp_drc,dp_dtc,dp_drf,dp_dtf); + i_pt++; + } + } +} + +// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper +// only +bool _mrcal_project_internal( // out + mrcal_point2_t* q, + + // Stored as a row-first array of shape (N,2,3). Each + // row lives in a mrcal_point3_t + mrcal_point3_t* dq_dp, + // core, distortions concatenated. Stored as a row-first + // array of shape (N,2,Nintrinsics). This is a DENSE array. + // High-parameter-count lens models have very sparse + // gradients here, and the internal project() function + // returns those sparsely. For now THIS function densifies + // all of these + double* dq_dintrinsics, + + // in + const mrcal_point3_t* p, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + + int Nintrinsics, + const mrcal_projection_precomputed_t* precomputed) +{ + if( dq_dintrinsics == NULL ) + { + for(int i=0; i dq_dintrinsics_pool_double(Ngradients); + int dq_dintrinsics_pool_int [1]; + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; // init to pacify compiler warning + + project( &q[i], + + dq_dintrinsics_pool_double.data(), + dq_dintrinsics_pool_int, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + NULL, NULL, NULL, dq_dp, NULL, + + // in + intrinsics, NULL, &frame, NULL, true, + lensmodel, precomputed, + 0.0, 0,0); + + int Ncore = 0; + if(dq_dfxy != NULL) + { + Ncore = 4; + + // fxy. off-diagonal elements are 0 + dq_dintrinsics[0*Nintrinsics + 0] = dq_dfxy[0]; + dq_dintrinsics[0*Nintrinsics + 1] = 0.0; + dq_dintrinsics[1*Nintrinsics + 0] = 0.0; + dq_dintrinsics[1*Nintrinsics + 1] = dq_dfxy[1]; + + // cxy. Identity + dq_dintrinsics[0*Nintrinsics + 2] = 1.0; + dq_dintrinsics[0*Nintrinsics + 3] = 0.0; + dq_dintrinsics[1*Nintrinsics + 2] = 0.0; + dq_dintrinsics[1*Nintrinsics + 3] = 1.0; + } + if( dq_dintrinsics_nocore != NULL ) + { + for(int i_xy=0; i_xy<2; i_xy++) + memcpy(&dq_dintrinsics[i_xy*Nintrinsics + Ncore], + &dq_dintrinsics_nocore[i_xy*(Nintrinsics-Ncore)], + (Nintrinsics-Ncore)*sizeof(double)); + } + if(gradient_sparse_meta.pool != NULL) + { + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3]) + + const int ivar0 = dq_dintrinsics_pool_int[0]; + const int len = gradient_sparse_meta.run_side_length; + + const double* ABCDx = &gradient_sparse_meta.pool[0]; + const double* ABCDy = &gradient_sparse_meta.pool[len]; + + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + const double* fxy = &intrinsics[0]; + for(int i_xy=0; i_xy<2; i_xy++) + for(int iy=0; iytype) || + lensmodel->type == MRCAL_LENSMODEL_PINHOLE)) + { + _mrcal_project_internal_opencv( q, NULL,NULL, + p, N, intrinsics, Nintrinsics); + return true; + } + + mrcal_projection_precomputed_t precomputed; + _mrcal_precompute_lensmodel_data(&precomputed, lensmodel); + + return + _mrcal_project_internal(q, dq_dp, dq_dintrinsics, + p, N, lensmodel, intrinsics, + Nintrinsics, &precomputed); +} + + +// Maps a set of distorted 2D imager points q to a 3D vector in camera +// coordinates that produced these pixel observations. The 3D vector is defined +// up-to-length. The returned vectors v are not normalized, and may have any +// length. +// +// This is the "reverse" direction, so an iterative nonlinear optimization is +// performed internally to compute this result. This is much slower than +// mrcal_project. For OpenCV distortions specifically, OpenCV has +// cvUndistortPoints() (and cv2.undistortPoints()), but these are inaccurate: +// https://github.com/opencv/opencv/issues/8811 +bool mrcal_unproject( // out + mrcal_point3_t* out, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics) +{ + + mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(lensmodel); + if(!meta.has_gradients) + { + MSG("mrcal_unproject(lensmodel='%s') is not yet implemented: we need gradients", + mrcal_lensmodel_name_unconfigured(lensmodel)); + return false; + } + + mrcal_projection_precomputed_t precomputed; + _mrcal_precompute_lensmodel_data(&precomputed, lensmodel); + + return _mrcal_unproject_internal(out, q, N, lensmodel, intrinsics, &precomputed); +} + +typedef struct +{ + const mrcal_lensmodel_t* lensmodel; + // core, distortions concatenated + const double* intrinsics; + const mrcal_projection_precomputed_t* precomputed; + const mrcal_point2_t* q; +} _unproject_callback_cookie_t; +static +void _unproject_callback(const double* u, + double* x, + double* J, + void* _cookie) +{ + _unproject_callback_cookie_t* cookie = (_unproject_callback_cookie_t*)_cookie; + + // u is the constant-fxy-cxy 2D stereographic + // projection of the hypothesis v. I unproject it stereographically, + // and project it using the actual model + mrcal_point2_t dv_du[3]; + mrcal_pose_t frame = {}; + mrcal_unproject_stereographic( &frame.t, dv_du, + (mrcal_point2_t*)u, 1, + cookie->intrinsics ); + + mrcal_point3_t dq_dtframe[2]; + mrcal_point2_t q_hypothesis; + project( &q_hypothesis, + NULL,NULL,NULL,NULL,NULL, + NULL, NULL, NULL, dq_dtframe, + NULL, + + // in + cookie->intrinsics, + NULL, + &frame, + NULL, + true, + cookie->lensmodel, cookie->precomputed, + 0.0, 0,0); + x[0] = q_hypothesis.x - cookie->q->x; + x[1] = q_hypothesis.y - cookie->q->y; + J[0*2 + 0] = + dq_dtframe[0].x*dv_du[0].x + + dq_dtframe[0].y*dv_du[1].x + + dq_dtframe[0].z*dv_du[2].x; + J[0*2 + 1] = + dq_dtframe[0].x*dv_du[0].y + + dq_dtframe[0].y*dv_du[1].y + + dq_dtframe[0].z*dv_du[2].y; + J[1*2 + 0] = + dq_dtframe[1].x*dv_du[0].x + + dq_dtframe[1].y*dv_du[1].x + + dq_dtframe[1].z*dv_du[2].x; + J[1*2 + 1] = + dq_dtframe[1].x*dv_du[0].y + + dq_dtframe[1].y*dv_du[1].y + + dq_dtframe[1].z*dv_du[2].y; +} + +// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper +// only +bool _mrcal_unproject_internal( // out + mrcal_point3_t* out, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + const mrcal_projection_precomputed_t* precomputed) +{ + // easy special-cases + if( lensmodel->type == MRCAL_LENSMODEL_PINHOLE ) + { + mrcal_unproject_pinhole(out, NULL, q, N, intrinsics); + return true; + } + if( lensmodel->type == MRCAL_LENSMODEL_STEREOGRAPHIC ) + { + mrcal_unproject_stereographic(out, NULL, q, N, intrinsics); + return true; + } + if( lensmodel->type == MRCAL_LENSMODEL_LONLAT ) + { + mrcal_unproject_lonlat(out, NULL, q, N, intrinsics); + return true; + } + if( lensmodel->type == MRCAL_LENSMODEL_LATLON ) + { + mrcal_unproject_latlon(out, NULL, q, N, intrinsics); + return true; + } + + if( lensmodel->type == MRCAL_LENSMODEL_CAHVORE ) + { + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + + for(int i = Nintrinsics-3; ixyz, NULL, + &v, + 1, + intrinsics ); + // MSG("init. out->xyz[]=(%g,%g)", out->x, out->y); + + + dogleg_parameters2_t dogleg_parameters; + dogleg_getDefaultParameters(&dogleg_parameters); + dogleg_parameters.dogleg_debug = 0; + + _unproject_callback_cookie_t cookie = + { .lensmodel = lensmodel, + .intrinsics = intrinsics, + .precomputed = precomputed, + .q = &q[i] }; + double norm2x = + dogleg_optimize_dense2(out->xyz, 2, 2, _unproject_callback, (void*)&cookie, + &dogleg_parameters, + NULL); + //This needs to be precise; if it isn't, I barf. Shouldn't happen + //very often + + static bool already_complained = false; + // MSG("norm2x = %g", norm2x); + if(norm2x/2.0 > 1e-4) + { + if(!already_complained) + { + // MSG("WARNING: I wasn't able to precisely compute some points. norm2x=%f. Returning nan for those. Will complain just once", + // norm2x); + already_complained = true; + } + double nan = strtod("NAN", NULL); + out->xyz[0] = nan; + out->xyz[1] = nan; + } + else + { + // out[0,1] is the stereographic representation of the observation + // vector using idealized fx,fy,cx,cy. This is already the right + // thing if we're reporting in 2d. Otherwise I need to unproject + + // This is the normal no-error path + mrcal_unproject_stereographic((mrcal_point3_t*)out, NULL, + (mrcal_point2_t*)out, 1, + intrinsics); + if(!model_supports_projection_behind_camera(lensmodel) && out->xyz[2] < 0.0) + { + out->xyz[0] *= -1.0; + out->xyz[1] *= -1.0; + out->xyz[2] *= -1.0; + } + } + + // Advance to the next point. Error or not + out++; + } + return true; +} + +// The following functions define/use the layout of the state vector. In general +// I do: +// +// intrinsics_cam0 +// intrinsics_cam1 +// intrinsics_cam2 +// ... +// extrinsics_cam1 +// extrinsics_cam2 +// extrinsics_cam3 +// ... +// frame0 +// frame1 +// frame2 +// .... +// calobject_warp0 +// calobject_warp1 +// ... + +// From real values to unit-scale values. Optimizer sees unit-scale values +static int pack_solver_state_intrinsics( // out + double* b, // subset based on problem_selections + + // in + const double* intrinsics, // ALL variables. Not a subset + const mrcal_lensmodel_t* lensmodel, + mrcal_problem_selections_t problem_selections, + int Ncameras_intrinsics ) +{ + int i_state = 0; + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0; + const int Ndistortions = Nintrinsics - Ncore; + + for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++) + { + if( problem_selections.do_optimize_intrinsics_core && Ncore ) + { + const mrcal_intrinsics_core_t* intrinsics_core = (const mrcal_intrinsics_core_t*)intrinsics; + b[i_state++] = intrinsics_core->focal_xy [0] / SCALE_INTRINSICS_FOCAL_LENGTH; + b[i_state++] = intrinsics_core->focal_xy [1] / SCALE_INTRINSICS_FOCAL_LENGTH; + b[i_state++] = intrinsics_core->center_xy[0] / SCALE_INTRINSICS_CENTER_PIXEL; + b[i_state++] = intrinsics_core->center_xy[1] / SCALE_INTRINSICS_CENTER_PIXEL; + } + + if( problem_selections.do_optimize_intrinsics_distortions ) + + for(int i = 0; ix2 / SCALE_CALOBJECT_WARP; + b[i_state++] = calobject_warp->y2 / SCALE_CALOBJECT_WARP; + } + + assert(i_state == Nstate_ref); +} + +// Same as above, but packs/unpacks a vector instead of structures +void mrcal_pack_solver_state_vector( // out, in + double* b, // FULL state on input, unitless + // state on output + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int Npoints_variable = Npoints - Npoints_fixed; + + int i_state = 0; + + i_state += pack_solver_state_intrinsics_subset_to_subset( b, + lensmodel, problem_selections, + Ncameras_intrinsics ); + + if( problem_selections.do_optimize_extrinsics ) + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + { + mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&b[i_state]); + + b[i_state++] = extrinsics_fromref->r.xyz[0] / SCALE_ROTATION_CAMERA; + b[i_state++] = extrinsics_fromref->r.xyz[1] / SCALE_ROTATION_CAMERA; + b[i_state++] = extrinsics_fromref->r.xyz[2] / SCALE_ROTATION_CAMERA; + + b[i_state++] = extrinsics_fromref->t.xyz[0] / SCALE_TRANSLATION_CAMERA; + b[i_state++] = extrinsics_fromref->t.xyz[1] / SCALE_TRANSLATION_CAMERA; + b[i_state++] = extrinsics_fromref->t.xyz[2] / SCALE_TRANSLATION_CAMERA; + } + + if( problem_selections.do_optimize_frames ) + { + for(int iframe = 0; iframe < Nframes; iframe++) + { + mrcal_pose_t* frames_toref = (mrcal_pose_t*)(&b[i_state]); + b[i_state++] = frames_toref->r.xyz[0] / SCALE_ROTATION_FRAME; + b[i_state++] = frames_toref->r.xyz[1] / SCALE_ROTATION_FRAME; + b[i_state++] = frames_toref->r.xyz[2] / SCALE_ROTATION_FRAME; + + b[i_state++] = frames_toref->t.xyz[0] / SCALE_TRANSLATION_FRAME; + b[i_state++] = frames_toref->t.xyz[1] / SCALE_TRANSLATION_FRAME; + b[i_state++] = frames_toref->t.xyz[2] / SCALE_TRANSLATION_FRAME; + } + + for(int i_point = 0; i_point < Npoints_variable; i_point++) + { + mrcal_point3_t* points = (mrcal_point3_t*)(&b[i_state]); + b[i_state++] = points->xyz[0] / SCALE_POSITION_POINT; + b[i_state++] = points->xyz[1] / SCALE_POSITION_POINT; + b[i_state++] = points->xyz[2] / SCALE_POSITION_POINT; + } + } + + if( has_calobject_warp(problem_selections,Nobservations_board) ) + { + mrcal_calobject_warp_t* calobject_warp = (mrcal_calobject_warp_t*)(&b[i_state]); + b[i_state++] = calobject_warp->x2 / SCALE_CALOBJECT_WARP; + b[i_state++] = calobject_warp->y2 / SCALE_CALOBJECT_WARP; + } +} + +static int unpack_solver_state_intrinsics( // out + + // Ncameras_intrinsics of these + double* intrinsics, // ALL variables. Not a subset. + // I don't touch the elemnents I'm + // not optimizing + + // in + const double* b, // subset based on problem_selections + const mrcal_lensmodel_t* lensmodel, + mrcal_problem_selections_t problem_selections, + int intrinsics_stride, + int Ncameras_intrinsics ) +{ + if( !problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions ) + return 0; + + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0; + + int i_state = 0; + for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++) + { + if( problem_selections.do_optimize_intrinsics_core && Ncore ) + { + intrinsics[icam_intrinsics*intrinsics_stride + 0] = b[i_state++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics[icam_intrinsics*intrinsics_stride + 1] = b[i_state++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics[icam_intrinsics*intrinsics_stride + 2] = b[i_state++] * SCALE_INTRINSICS_CENTER_PIXEL; + intrinsics[icam_intrinsics*intrinsics_stride + 3] = b[i_state++] * SCALE_INTRINSICS_CENTER_PIXEL; + } + + if( problem_selections.do_optimize_intrinsics_distortions ) + { + for(int i = 0; ir.xyz[0] = b[i_state++] * SCALE_ROTATION_CAMERA; + extrinsic->r.xyz[1] = b[i_state++] * SCALE_ROTATION_CAMERA; + extrinsic->r.xyz[2] = b[i_state++] * SCALE_ROTATION_CAMERA; + + extrinsic->t.xyz[0] = b[i_state++] * SCALE_TRANSLATION_CAMERA; + extrinsic->t.xyz[1] = b[i_state++] * SCALE_TRANSLATION_CAMERA; + extrinsic->t.xyz[2] = b[i_state++] * SCALE_TRANSLATION_CAMERA; + return i_state; +} + +static int unpack_solver_state_framert_one(// out + mrcal_pose_t* frame, + + // in + const double* b) +{ + int i_state = 0; + frame->r.xyz[0] = b[i_state++] * SCALE_ROTATION_FRAME; + frame->r.xyz[1] = b[i_state++] * SCALE_ROTATION_FRAME; + frame->r.xyz[2] = b[i_state++] * SCALE_ROTATION_FRAME; + + frame->t.xyz[0] = b[i_state++] * SCALE_TRANSLATION_FRAME; + frame->t.xyz[1] = b[i_state++] * SCALE_TRANSLATION_FRAME; + frame->t.xyz[2] = b[i_state++] * SCALE_TRANSLATION_FRAME; + return i_state; + +} + +static int unpack_solver_state_point_one(// out + mrcal_point3_t* point, + + // in + const double* b) +{ + int i_state = 0; + point->xyz[0] = b[i_state++] * SCALE_POSITION_POINT; + point->xyz[1] = b[i_state++] * SCALE_POSITION_POINT; + point->xyz[2] = b[i_state++] * SCALE_POSITION_POINT; + return i_state; +} + +static int unpack_solver_state_calobject_warp(// out + mrcal_calobject_warp_t* calobject_warp, + + // in + const double* b) +{ + int i_state = 0; + calobject_warp->x2 = b[i_state++] * SCALE_CALOBJECT_WARP; + calobject_warp->y2 = b[i_state++] * SCALE_CALOBJECT_WARP; + return i_state; +} + +// From unit-scale values to real values. Optimizer sees unit-scale values +static void unpack_solver_state( // out + + // ALL intrinsics; whether we're optimizing + // them or not. Don't touch the parts of this + // array we're not optimizing + double* intrinsics_all, // Ncameras_intrinsics of these + + mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these + mrcal_pose_t* frames_toref, // Nframes of these + mrcal_point3_t* points, // Npoints of these + mrcal_calobject_warp_t* calobject_warp, // 1 of these + + // in + const double* b, + const mrcal_lensmodel_t* lensmodel, + mrcal_problem_selections_t problem_selections, + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, int Npoints_variable, + int Nobservations_board, + int Nstate_ref) +{ + int i_state = unpack_solver_state_intrinsics(intrinsics_all, + b, lensmodel, problem_selections, + mrcal_lensmodel_num_params(lensmodel), + Ncameras_intrinsics); + + if( problem_selections.do_optimize_extrinsics ) + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &b[i_state] ); + + if( problem_selections.do_optimize_frames ) + { + for(int iframe = 0; iframe < Nframes; iframe++) + i_state += unpack_solver_state_framert_one( &frames_toref[iframe], &b[i_state] ); + for(int i_point = 0; i_point < Npoints_variable; i_point++) + i_state += unpack_solver_state_point_one( &points[i_point], &b[i_state] ); + } + + if( has_calobject_warp(problem_selections,Nobservations_board) ) + i_state += unpack_solver_state_calobject_warp(calobject_warp, &b[i_state]); + + assert(i_state == Nstate_ref); +} +// Same as above, but packs/unpacks a vector instead of structures +void mrcal_unpack_solver_state_vector( // out, in + double* b, // unitless state on input, + // scaled, meaningful state on + // output + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int Npoints_variable = Npoints - Npoints_fixed; + + int i_state = + unpack_solver_state_intrinsics_subset_to_subset(b, + lensmodel, problem_selections, + Ncameras_intrinsics); + + if( problem_selections.do_optimize_extrinsics ) + { + mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&b[i_state]); + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &b[i_state] ); + } + + if( problem_selections.do_optimize_frames ) + { + mrcal_pose_t* frames_toref = (mrcal_pose_t*)(&b[i_state]); + for(int iframe = 0; iframe < Nframes; iframe++) + i_state += unpack_solver_state_framert_one( &frames_toref[iframe], &b[i_state] ); + mrcal_point3_t* points = (mrcal_point3_t*)(&b[i_state]); + for(int i_point = 0; i_point < Npoints_variable; i_point++) + i_state += unpack_solver_state_point_one( &points[i_point], &b[i_state] ); + } + if( has_calobject_warp(problem_selections,Nobservations_board) ) + { + mrcal_calobject_warp_t* calobject_warp = (mrcal_calobject_warp_t*)(&b[i_state]); + i_state += unpack_solver_state_calobject_warp(calobject_warp, &b[i_state]); + } +} + +int mrcal_state_index_intrinsics(int icam_intrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(Ncameras_intrinsics <= 0) + return -1; + int Nintrinsics = mrcal_num_intrinsics_optimization_params(problem_selections, lensmodel); + if(Nintrinsics <= 0) + return -1; + if(!(0 <= icam_intrinsics && icam_intrinsics < Ncameras_intrinsics)) + return -1; + return icam_intrinsics * Nintrinsics; +} + +int mrcal_num_states_intrinsics(int Ncameras_intrinsics, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + return + Ncameras_intrinsics * + mrcal_num_intrinsics_optimization_params(problem_selections, lensmodel); +} + +int mrcal_state_index_extrinsics(int icam_extrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(Ncameras_extrinsics <= 0) + return -1; + if(!problem_selections.do_optimize_extrinsics) + return -1; + if(!(0 <= icam_extrinsics && icam_extrinsics < Ncameras_extrinsics)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + (icam_extrinsics*6); +} + +int mrcal_num_states_extrinsics(int Ncameras_extrinsics, + mrcal_problem_selections_t problem_selections) +{ + return problem_selections.do_optimize_extrinsics ? (6*Ncameras_extrinsics) : 0; +} + +int mrcal_state_index_frames(int iframe, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(Nframes <= 0) + return -1; + if(!problem_selections.do_optimize_frames) + return -1; + if(!(0 <= iframe && iframe < Nframes)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + (iframe*6); +} + +int mrcal_num_states_frames(int Nframes, + mrcal_problem_selections_t problem_selections) +{ + return problem_selections.do_optimize_frames ? (6*Nframes) : 0; +} + +int mrcal_state_index_points(int i_point, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int Npoints_variable = Npoints - Npoints_fixed; + + if(Npoints_variable <= 0) + return -1; + if(!problem_selections.do_optimize_frames) + return -1; + if(!(0 <= i_point && i_point < Npoints_variable)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + mrcal_num_states_frames (Nframes, + problem_selections) + + (i_point*3); +} + +int mrcal_num_states_points(int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections) +{ + int Npoints_variable = Npoints - Npoints_fixed; + return problem_selections.do_optimize_frames ? (Npoints_variable*3) : 0; +} + +int mrcal_state_index_calobject_warp(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(!has_calobject_warp(problem_selections,Nobservations_board)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + mrcal_num_states_frames (Nframes, + problem_selections) + + mrcal_num_states_points (Npoints, Npoints_fixed, + problem_selections); +} + +int mrcal_num_states_calobject_warp(mrcal_problem_selections_t problem_selections, + int Nobservations_board) +{ + if(has_calobject_warp(problem_selections,Nobservations_board)) + return MRCAL_NSTATE_CALOBJECT_WARP; + return 0; +} + +// Reports the icam_extrinsics corresponding to a given icam_intrinsics. +// +// If we're solving a vanilla calibration problem (stationary cameras observing +// a moving calibration object), each camera has a unique intrinsics index and a +// unique extrinsics index. This function reports the latter, given the former. +// +// On success, the result is written to *icam_extrinsics, and we return true. If +// the given camera is at the reference coordinate system, it has no extrinsics, +// and we report -1. +// +// If we have moving cameras (NOT a vanilla calibration problem), there isn't a +// single icam_extrinsics for a given icam_intrinsics, and we report an error by +// returning false +static +bool _corresponding_icam_extrinsics__check( const mrcal_camera_index_t* icam, int i, + int* icam_map_to_extrinsics, + int* icam_map_to_intrinsics, + const char* what) +{ + int icam_intrinsics = icam->intrinsics; + int icam_extrinsics = icam->extrinsics; + + if(icam_extrinsics < 0) icam_extrinsics = -1; + + if(icam_map_to_intrinsics[icam_extrinsics+1] == -100) + icam_map_to_intrinsics[icam_extrinsics+1] = icam_intrinsics; + else if(icam_map_to_intrinsics[icam_extrinsics+1] != icam_intrinsics) + { + MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem: %s observation %d has icam_intrinsics,icam_extrinsics %d,%d while I saw %d,%d previously", + what, i, + icam_map_to_intrinsics[icam_extrinsics+1], icam_extrinsics, + icam_intrinsics, icam_extrinsics); + return false; + } + + if(icam_map_to_extrinsics[icam_intrinsics] == -100) + icam_map_to_extrinsics[icam_intrinsics] = icam_extrinsics; + else if(icam_map_to_extrinsics[icam_intrinsics] != icam_extrinsics) + { + MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem: %s observation %d has icam_intrinsics,icam_extrinsics %d,%d while I saw %d,%d previously", + what, i, + icam_intrinsics, icam_map_to_extrinsics[icam_intrinsics], + icam_intrinsics, icam_extrinsics); + return false; + } + return true; +} +bool mrcal_corresponding_icam_extrinsics(// out + int* icam_extrinsics, + + // in + int icam_intrinsics, + int Ncameras_intrinsics, + int Ncameras_extrinsics, + int Nobservations_board, + const mrcal_observation_board_t* observations_board, + int Nobservations_point, + const mrcal_observation_point_t* observations_point) +{ + if( !(Ncameras_intrinsics == Ncameras_extrinsics || + Ncameras_intrinsics == Ncameras_extrinsics+1 ) ) + { + MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem (stationary cameras, cam0 is reference)"); + return false; + } + + std::vector icam_map_to_extrinsics(Ncameras_intrinsics); + std::vector icam_map_to_intrinsics(Ncameras_extrinsics+1); + for(int i=0; i k stdevs past the mean. I make + // a broad assumption that the error distribution is normally-distributed, + // with mean 0. This is reasonable because this function is applied after + // running the optimization. + // + // The threshold is based on the stdev of my observed residuals + // + // I have two separate thresholds. If any measurements are worse than the + // higher threshold, then I will need to reoptimize, so I throw out some + // extra points: all points worse than the lower threshold. This serves to + // reduce the required re-optimizations + + const double k0 = 4.0; + const double k1 = 5.0; + + *Noutliers_board = 0; + int Ninliers_board = 0; + + *Noutliers_triangulated_point = 0; + int Ninliers_triangulated_point = 0; + + const int imeasurement_board0 = + mrcal_measurement_index_boards(0, + Nobservations_board, + Nobservations_point, + calibration_object_width_n, + calibration_object_height_n); + const int imeasurement_point_triangulated0 = + mrcal_measurement_index_points_triangulated(0, + Nobservations_board, + Nobservations_point, + + observations_point_triangulated, + Nobservations_point_triangulated, + + calibration_object_width_n, + calibration_object_height_n); + + + // just in case + if(Nobservations_point_triangulated <= 0) + { + Nobservations_point_triangulated = 0; + observations_point_triangulated = NULL; + } + + const double* x_boards = + &x_measurements[ imeasurement_board0 ]; + const double* x_point_triangulated = + &x_measurements[ imeasurement_point_triangulated0 ]; + +#define LOOP_BOARD_OBSERVATION(extra_while_condition) \ + for(int i_observation_board=0, i_pt_board = 0; \ + i_observation_boardicam.intrinsics; \ + for(int i_pt=0; \ + i_pt < calibration_object_width_n*calibration_object_height_n; \ + i_pt++, i_pt_board++) + +#define LOOP_BOARD_FEATURE_HEADER() \ + const mrcal_point3_t* pt_observed = &observations_board_pool[i_pt_board]; \ + double* weight = &observations_board_pool[i_pt_board].z; + +#define LOOP_TRIANGULATED_POINT0(extra_while_condition) \ + for(int i0 = 0, \ + imeasurement_point_triangulated = 0; \ + i0 < Nobservations_point_triangulated && extra_while_condition; \ + i0++) + +#define LOOP_TRIANGULATED_POINT1() \ + mrcal_observation_point_triangulated_t* pt0 = \ + &observations_point_triangulated[i0]; \ + if(pt0->last_in_set) \ + continue; \ + \ + int i1 = i0+1; + +#define LOOP_TRIANGULATED_POINT_HEADER() \ + mrcal_observation_point_triangulated_t* pt1 = \ + &observations_point_triangulated[i1]; + +#define LOOP_TRIANGULATED_POINT_FOOTER() \ + imeasurement_point_triangulated++; \ + if(pt1->last_in_set) \ + break; \ + i1++; + + + + + + /////////////// Compute the variance to set the threshold + bool foundNewOutliers = false; + + double var = 0.0; + LOOP_BOARD_OBSERVATION(true) + { + LOOP_BOARD_FEATURE() + { + LOOP_BOARD_FEATURE_HEADER(); + + if(*weight <= 0.0) + { + (*Noutliers_board)++; + continue; + } + + double dx = x_boards[2*i_pt_board + 0]; + double dy = x_boards[2*i_pt_board + 1]; + var += dx*dx + dy*dy; + Ninliers_board++; + } + } + + MSG("I started with %d board outliers", *Noutliers_board); + int Nmeasurements_outliers_triangulated_start = 0; + + LOOP_TRIANGULATED_POINT0(true) + { + LOOP_TRIANGULATED_POINT1(); + + const mrcal_point3_t* v0 = &pt0->px; + const mrcal_point3_t* t_r0; + mrcal_point3_t _t_r0; + const mrcal_point3_t* v0_ref; + mrcal_point3_t _v0_ref; + + const int icam_extrinsics0 = pt0->icam.extrinsics; + if( icam_extrinsics0 >= 0 ) + { + const mrcal_pose_t* rt_0r = &rt_extrinsics_fromref[icam_extrinsics0]; + const double* r_0r = &rt_0r->r.xyz[0]; + const double* t_0r = &rt_0r->t.xyz[0]; + + t_r0 = &_t_r0; + v0_ref = &_v0_ref; + + mrcal_rotate_point_r_inverted(_t_r0.xyz, NULL,NULL, + r_0r, t_0r); + for(int i=0; i<3; i++) + _t_r0.xyz[i] *= -1.; + + mrcal_rotate_point_r_inverted(_v0_ref.xyz, NULL,NULL, + r_0r, v0->xyz); + } + else + { + t_r0 = NULL; + v0_ref = v0; + } + + + while(true) + { + LOOP_TRIANGULATED_POINT_HEADER(); + + /////////////// divergent triangulated observations are DEFINITELY outliers + if(pt0->outlier || pt1->outlier) + Nmeasurements_outliers_triangulated_start++; + else + { + const mrcal_point3_t* v1 = &pt1->px; + + const mrcal_point3_t* t_10; + mrcal_point3_t _t_10; + const mrcal_point3_t* v0_cam1; + mrcal_point3_t _v0_cam1; + + const int icam_extrinsics1 = pt1->icam.extrinsics; + if( icam_extrinsics1 >= 0 ) + { + const mrcal_pose_t* rt_1r = &rt_extrinsics_fromref[icam_extrinsics1]; + + v0_cam1 = &_v0_cam1; + + if( icam_extrinsics0 >= 0 ) + { + t_10 = &_t_10; + mrcal_transform_point_rt( &_t_10.xyz[0], NULL, NULL, + &rt_1r->r.xyz[0], &t_r0->xyz[0] ); + } + else + { + // t_r0 = 0 -> + // + // t_10 = R_1r*t_r0 + t_1r = + // = R_1r*0 + t_1r = + // = t_1r + t_10 = &rt_1r->t; + } + + mrcal_rotate_point_r( &_v0_cam1.xyz[0], NULL, NULL, + &rt_1r->r.xyz[0], &v0_ref->xyz[0] ); + } + else + { + // rt_1r = 0 -> + // + // t_10 = R_1r*t_r0 + t_1r = + // = t_r0 + t_10 = t_r0; + // At most one camera can sit at the reference. So if I'm + // here, I know that t_r0 != NULL and thus t_10 != NULL + + v0_cam1 = v0_ref; + } + if(!_mrcal_triangulate_leecivera_mid2_is_convergent(v1, v0_cam1, t_10)) + { + // Outlier. I don't know which observations was the broken + // one, so I mark them both + pt0->outlier = true; + pt1->outlier = true; + foundNewOutliers = true; +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: outliers should not be marked in this first loop. This should happen in the following loop. Putting it here breaks the logic" +#endif + + + // There are a lot of these, so I'm disabling this print for + // now, to avoid spamming the terminal + // + // MSG("New divergent-feature outliers found: measurement %d observation (%d,%d)", + // imeasurement_point_triangulated0 + imeasurement_point_triangulated, + // i0, i1); + + } + } + // just marked divergent triangulations as outliers + + + if(pt0->outlier || pt1->outlier) + (*Noutliers_triangulated_point)++; + else + { + var += + x_point_triangulated[imeasurement_point_triangulated] * + x_point_triangulated[imeasurement_point_triangulated]; + Ninliers_triangulated_point++; + } + LOOP_TRIANGULATED_POINT_FOOTER(); + } + } + if(Nobservations_point_triangulated > 0) + { + MSG("I started with %d triangulated outliers", Nmeasurements_outliers_triangulated_start); + MSG("I started with %d triangulated outliers", *Noutliers_triangulated_point); + } + var /= (double)(Ninliers_board*2 + Ninliers_triangulated_point); + // MSG("Outlier rejection sees stdev = %f", sqrt(var)); + + ///////////// Any new outliers found? + LOOP_BOARD_OBSERVATION(!foundNewOutliers) + { + LOOP_BOARD_FEATURE() + { + LOOP_BOARD_FEATURE_HEADER(); + + if(*weight <= 0.0) + continue; + + double dx = x_boards[2*i_pt_board + 0]; + double dy = x_boards[2*i_pt_board + 1]; + // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma + // -> x^2 > k^2 var + if(dx*dx > k1*k1*var || + dy*dy > k1*k1*var ) + { + // MSG("Feature %d looks like an outlier. x/y are %f/%f stdevs off mean (assumed 0). Observed stdev: %f, limit: %f", + // i_pt_board, + // dx/sqrt(var), + // dy/sqrt(var), + // sqrt(var), + // k1); + foundNewOutliers = true; + break; + } + } + } + LOOP_TRIANGULATED_POINT0(!foundNewOutliers) + { + LOOP_TRIANGULATED_POINT1(); + while(true) + { + LOOP_TRIANGULATED_POINT_HEADER(); + + if(!pt0->outlier && !pt1->outlier) + { + double dx = x_point_triangulated[imeasurement_point_triangulated]; + + // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma + // -> x^2 > k^2 var + if(dx*dx > k1*k1*var ) + { + foundNewOutliers = true; + break; + } + } + LOOP_TRIANGULATED_POINT_FOOTER(); + } + } + if(!foundNewOutliers) + return false; + + // I have new outliers: some measurements were found past the threshold. I + // throw out a bit extra to leave some margin so that the next + // re-optimization would hopefully be the last. + LOOP_BOARD_OBSERVATION(true) + { + int Npt_inlier = 0; + int Npt_outlier = 0; + LOOP_BOARD_FEATURE() + { + LOOP_BOARD_FEATURE_HEADER(); + if(*weight <= 0.0) + { + Npt_outlier++; + continue; + } + Npt_inlier++; + + double dx = x_boards[2*i_pt_board + 0]; + double dy = x_boards[2*i_pt_board + 1]; + // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma + // -> x^2 > k^2 var + if(dx*dx > k0*k0*var || + dy*dy > k0*k0*var ) + { + *weight *= -1.0; + (*Noutliers_board)++; + } + } + + if(Npt_inlier < 3) + MSG("WARNING: Board observation %d (icam_intrinsics=%d, icam_extrinsics=%d, iframe=%d) had almost all of its points thrown out as outliers: only %d/%d remain. CHOLMOD is about to complain about a non-positive-definite JtJ. Something is wrong with this observation", + i_observation_board, + observation->icam.intrinsics, + observation->icam.extrinsics, + observation->iframe, + Npt_inlier, + Npt_inlier + Npt_outlier); + } + LOOP_TRIANGULATED_POINT0(true) + { + LOOP_TRIANGULATED_POINT1(); + while(true) + { + LOOP_TRIANGULATED_POINT_HEADER(); + + if(!pt0->outlier && !pt1->outlier) + { + double dx = x_point_triangulated[imeasurement_point_triangulated]; + + // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma + // -> x^2 > k^2 var + if(dx*dx > k0*k0*var ) + { + // Outlier. I don't know which observations was the broken + // one, so I mark them both + pt0->outlier = true; + pt1->outlier = true; + + (*Noutliers_triangulated_point)++; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: outliers not returned to the caller yet, so I simply print them out here" +#endif + MSG("New outliers found: measurement %d observation (%d,%d)", + imeasurement_point_triangulated0 + imeasurement_point_triangulated, + i0, i1); + } + } + + LOOP_TRIANGULATED_POINT_FOOTER(); + } + } + + return true; + +#undef LOOP_BOARD_OBSERVATION +#undef LOOP_BOARD_FEATURE +#undef LOOP_BOARD_FEATURE_HEADER +#undef LOOP_TRIANGULATED_POINT0 +#undef LOOP_TRIANGULATED_POINT1 +#undef LOOP_TRIANGULATED_POINT_HEADER +#undef LOOP_TRIANGULATED_POINT_FOOTER +} + + +typedef struct +{ + // these are all UNPACKED + const double* intrinsics; // Ncameras_intrinsics * NlensParams of these + const mrcal_pose_t* extrinsics_fromref; // Ncameras_extrinsics of these. Transform FROM the reference frame + const mrcal_pose_t* frames_toref; // Nframes of these. Transform TO the reference frame + const mrcal_point3_t* points; // Npoints of these. In the reference frame + const mrcal_calobject_warp_t* calobject_warp; // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + // in + int Ncameras_intrinsics, Ncameras_extrinsics, Nframes; + int Npoints, Npoints_fixed; + + const mrcal_observation_board_t* observations_board; + const mrcal_point3_t* observations_board_pool; + int Nobservations_board; + + const mrcal_observation_point_t* observations_point; + const mrcal_point3_t* observations_point_pool; + int Nobservations_point; + + const mrcal_observation_point_triangulated_t* observations_point_triangulated; + int Nobservations_point_triangulated; + + bool verbose; + + mrcal_lensmodel_t lensmodel; + mrcal_projection_precomputed_t precomputed; + const int* imagersizes; // Ncameras_intrinsics*2 of these + + mrcal_problem_selections_t problem_selections; + const mrcal_problem_constants_t* problem_constants; + + double calibration_object_spacing; + int calibration_object_width_n; + int calibration_object_height_n; + + const int Nmeasurements, N_j_nonzero, Nintrinsics; +} callback_context_t; + +static +void penalty_range_normalization(// out + double* penalty, double* dpenalty_ddistsq, + + // in + // SIGNED distance. <0 means "behind the camera" + const double distsq, + const callback_context_t* ctx, + const double weight) +{ + const double maxsq = ctx->problem_constants->point_max_range*ctx->problem_constants->point_max_range; + if(distsq > maxsq) + { + *penalty = weight * (distsq/maxsq - 1.0); + *dpenalty_ddistsq = weight*(1. / maxsq); + return; + } + + const double minsq = ctx->problem_constants->point_min_range*ctx->problem_constants->point_min_range; + if(distsq < minsq) + { + // too close OR behind the camera + *penalty = weight*(1.0 - distsq/minsq); + *dpenalty_ddistsq = weight*(-1. / minsq); + return; + } + + *penalty = *dpenalty_ddistsq = 0.0; +} +static +void optimizer_callback(// input state + const double* packed_state, + + // output measurements + double* x, + + // Jacobian + cholmod_sparse* Jt, + + const callback_context_t* ctx) +{ + double norm2_error = 0.0; + + int iJacobian = 0; + int iMeasurement = 0; + + int* Jrowptr = Jt ? (int*) Jt->p : NULL; + int* Jcolidx = Jt ? (int*) Jt->i : NULL; + double* Jval = Jt ? (double*)Jt->x : NULL; +#define STORE_JACOBIAN(col, g) \ + do \ + { \ + if(Jt) { \ + Jcolidx[ iJacobian ] = col; \ + Jval [ iJacobian ] = g; \ + } \ + iJacobian++; \ + } while(0) +#define STORE_JACOBIAN2(col0, g0, g1) \ + do \ + { \ + if(Jt) { \ + Jcolidx[ iJacobian+0 ] = col0+0; \ + Jval [ iJacobian+0 ] = g0; \ + Jcolidx[ iJacobian+1 ] = col0+1; \ + Jval [ iJacobian+1 ] = g1; \ + } \ + iJacobian += 2; \ + } while(0) +#define STORE_JACOBIAN3(col0, g0, g1, g2) \ + do \ + { \ + if(Jt) { \ + Jcolidx[ iJacobian+0 ] = col0+0; \ + Jval [ iJacobian+0 ] = g0; \ + Jcolidx[ iJacobian+1 ] = col0+1; \ + Jval [ iJacobian+1 ] = g1; \ + Jcolidx[ iJacobian+2 ] = col0+2; \ + Jval [ iJacobian+2 ] = g2; \ + } \ + iJacobian += 3; \ + } while(0) +#define STORE_JACOBIAN_N(col0, g0, scale, N) \ + do \ + { \ + if(Jt) { \ + for(int i=0; ilensmodel) ? 4 : 0; + int Ncore_state = (modelHasCore_fxfycxcy(&ctx->lensmodel) && + ctx->problem_selections.do_optimize_intrinsics_core) ? 4 : 0; + + // If I'm locking down some parameters, then the state vector contains a + // subset of my data. I reconstitute the intrinsics and extrinsics here. + // I do the frame poses later. This is a good way to do it if I have few + // cameras. With many cameras (this will be slow) + + // WARNING: sparsify this. This is potentially a BIG thing on the stack + std::vector> intrinsics_all(ctx->Ncameras_intrinsics, std::vector(ctx->Nintrinsics)); + std::vector camera_rt(ctx->Ncameras_extrinsics); + + mrcal_calobject_warp_t calobject_warp_local = {}; + const int i_var_calobject_warp = + mrcal_state_index_calobject_warp(ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + if(has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board)) + unpack_solver_state_calobject_warp(&calobject_warp_local, &packed_state[i_var_calobject_warp]); + else if(ctx->calobject_warp != NULL) + calobject_warp_local = *ctx->calobject_warp; + + for(int icam_intrinsics=0; + icam_intrinsicsNcameras_intrinsics; + icam_intrinsics++) + { + // Construct the FULL intrinsics vector, based on either the + // optimization vector or the inputs, depending on what we're optimizing + double* intrinsics_here = &intrinsics_all[icam_intrinsics][0]; + + int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + if(Ncore) + { + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + intrinsics_here[0] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics_here[1] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics_here[2] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_CENTER_PIXEL; + intrinsics_here[3] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_CENTER_PIXEL; + } + else + memcpy( intrinsics_here, + &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics], + Ncore*sizeof(double) ); + } + int nDistortion = ctx->Nintrinsics-Ncore; + if (nDistortion) { + double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore]; + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + for(int i = 0; iNintrinsics-Ncore; i++) + distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION; + } + else { + memcpy( distortions_here, + &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore], + (ctx->Nintrinsics-Ncore)*sizeof(double) ); + } + } + } + for(int icam_extrinsics=0; + icam_extrinsicsNcameras_extrinsics; + icam_extrinsics++) + { + if( icam_extrinsics < 0 ) continue; + + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + if(ctx->problem_selections.do_optimize_extrinsics) + unpack_solver_state_extrinsics_one(&camera_rt[icam_extrinsics], &packed_state[i_var_camera_rt]); + else + memcpy(&camera_rt[icam_extrinsics], &ctx->extrinsics_fromref[icam_extrinsics], sizeof(mrcal_pose_t)); + } + + int i_feature = 0; + for(int i_observation_board = 0; + i_observation_board < ctx->Nobservations_board; + i_observation_board++) + { + const mrcal_observation_board_t* observation = &ctx->observations_board[i_observation_board]; + + const int icam_intrinsics = observation->icam.intrinsics; + const int icam_extrinsics = observation->icam.extrinsics; + const int iframe = observation->iframe; + + + // Some of these are bogus if problem_selections says they're inactive + const int i_var_frame_rt = + mrcal_state_index_frames(iframe, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + mrcal_pose_t frame_rt; + if(ctx->problem_selections.do_optimize_frames) + unpack_solver_state_framert_one(&frame_rt, &packed_state[i_var_frame_rt]); + else + memcpy(&frame_rt, &ctx->frames_toref[iframe], sizeof(mrcal_pose_t)); + + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + // invalid if icam_extrinsics < 0, but unused in that case + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + // these are computed in respect to the real-unit parameters, + // NOT the unit-scale parameters used by the optimizer + std::vector dq_drcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dtcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_drframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dtframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector q_hypothesis (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + // I get the intrinsics gradients in separate arrays, possibly sparsely. + // All the data lives in dq_dintrinsics_pool_double[], with the other data + // indicating the meaning of the values in the pool. + // + // dq_dfxy serves a special-case for a perspective core. Such models + // are very common, and they have x = fx vx/vz + cx and y = fy vy/vz + + // cy. So x depends on fx and NOT on fy, and similarly for y. Similar + // for cx,cy, except we know the gradient value beforehand. I support + // this case explicitly here. I store dx/dfx and dy/dfy; no cross terms + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + + std::vector dq_dintrinsics_pool_double(ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients); + std::vector dq_dintrinsics_pool_int (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; + + int splined_intrinsics_grad_irun = 0; + + bool camera_at_identity = icam_extrinsics < 0; + project(q_hypothesis.data(), + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_int.data() : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + ctx->problem_selections.do_optimize_extrinsics ? + dq_drcamera.data() : NULL, + ctx->problem_selections.do_optimize_extrinsics ? + dq_dtcamera.data() : NULL, + ctx->problem_selections.do_optimize_frames ? + dq_drframe.data() : NULL, + ctx->problem_selections.do_optimize_frames ? + dq_dtframe.data() : NULL, + has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ? + dq_dcalobject_warp.data() : NULL, + + // input + intrinsics_all[icam_intrinsics].data(), + camera_at_identity ? NULL : &camera_rt[icam_extrinsics], &frame_rt, + ctx->calobject_warp == NULL ? NULL : &calobject_warp_local, + camera_at_identity, + &ctx->lensmodel, &ctx->precomputed, + ctx->calibration_object_spacing, + ctx->calibration_object_width_n, + ctx->calibration_object_height_n); + + for(int i_pt=0; + i_pt < ctx->calibration_object_width_n*ctx->calibration_object_height_n; + i_pt++, i_feature++) + { + const mrcal_point3_t* qx_qy_w__observed = &ctx->observations_board_pool[i_feature]; + double weight = qx_qy_w__observed->z; + + if(weight >= 0.0) + { + // I have my two measurements (dx, dy). I propagate their + // gradient and store them + for( int i_xy=0; i_xy<2; i_xy++ ) + { + const double err = (q_hypothesis[i_pt].xy[i_xy] - qx_qy_w__observed->xyz[i_xy]) * weight; + + if(ctx->verbose) + MSG("obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g", + i_observation_board, iframe, icam_intrinsics, icam_extrinsics, i_pt, err); + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = err; + norm2_error += err*err; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + // fx,fy. x depends on fx only. y depends on fy only + STORE_JACOBIAN( i_var_intrinsics + i_xy, + dq_dfxy[i_pt*2 + i_xy] * + weight * SCALE_INTRINSICS_FOCAL_LENGTH ); + + // cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, + weight * SCALE_INTRINSICS_CENTER_PIXEL ); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if(gradient_sparse_meta.pool != NULL) + { + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3]) + const int ivar0 = dq_dintrinsics_pool_int[splined_intrinsics_grad_irun] - + ( ctx->problem_selections.do_optimize_intrinsics_core ? 0 : 4 ); + + const int len = gradient_sparse_meta.run_side_length; + const double* ABCDx = &gradient_sparse_meta.pool[len*2*splined_intrinsics_grad_irun + 0]; + const double* ABCDy = &gradient_sparse_meta.pool[len*2*splined_intrinsics_grad_irun + len]; + + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + const double* fxy = &intrinsics_all[icam_intrinsics][0]; + + for(int iy=0; iyNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, + dq_dintrinsics_nocore[i_pt*2*(ctx->Nintrinsics-Ncore) + + i_xy*(ctx->Nintrinsics-Ncore) + + i] * + weight * SCALE_DISTORTION ); + } + } + + if( ctx->problem_selections.do_optimize_extrinsics ) + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, + dq_drcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_CAMERA); + STORE_JACOBIAN3( i_var_camera_rt + 3, + dq_dtcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_CAMERA); + } + + if( ctx->problem_selections.do_optimize_frames ) + { + STORE_JACOBIAN3( i_var_frame_rt + 0, + dq_drframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_FRAME, + dq_drframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_FRAME, + dq_drframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_FRAME); + STORE_JACOBIAN3( i_var_frame_rt + 3, + dq_dtframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_FRAME, + dq_dtframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_FRAME, + dq_dtframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_FRAME); + } + + if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ) + { + STORE_JACOBIAN_N( i_var_calobject_warp, + dq_dcalobject_warp[i_pt * 2 + i_xy].values, + weight * SCALE_CALOBJECT_WARP, + MRCAL_NSTATE_CALOBJECT_WARP); + } + + iMeasurement++; + } + } + else + { + // Outlier. + + // This is arbitrary. I'm skipping this observation, so I don't + // touch the projection results, and I set the measurement and + // all its gradients to 0. I need to have SOME dependency on the + // frame parameters to ensure a full-rank Hessian, so if we're + // skipping all observations for this frame the system will + // become singular. I don't currently handle this. libdogleg + // will complain loudly, and add small diagonal L2 + // regularization terms + for( int i_xy=0; i_xy<2; i_xy++ ) + { + const double err = 0.0; + + if(ctx->verbose) + MSG( "obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g", + i_observation_board, iframe, icam_intrinsics, icam_extrinsics, i_pt, err); + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = err; + norm2_error += err*err; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + STORE_JACOBIAN( i_var_intrinsics + i_xy, 0.0 ); + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, 0.0 ); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if(gradient_sparse_meta.pool != NULL) + { + const int ivar0 = dq_dintrinsics_pool_int[splined_intrinsics_grad_irun] - + ( ctx->problem_selections.do_optimize_intrinsics_core ? 0 : 4 ); + const int len = gradient_sparse_meta.run_side_length; + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + + for(int iy=0; iyNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, 0.0 ); + } + } + + if( ctx->problem_selections.do_optimize_extrinsics ) + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, 0.0, 0.0, 0.0); + STORE_JACOBIAN3( i_var_camera_rt + 3, 0.0, 0.0, 0.0); + } + + if( ctx->problem_selections.do_optimize_frames ) + { + // Arbitrary differences between the dimensions to keep + // my Hessian non-singular. This is 100% arbitrary. I'm + // skipping these measurements so these variables + // actually don't affect the computation at all + STORE_JACOBIAN3( i_var_frame_rt + 0, 0,0,0); + STORE_JACOBIAN3( i_var_frame_rt + 3, 0,0,0); + } + + if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ) + STORE_JACOBIAN_N( i_var_calobject_warp, + (double*)NULL, 0.0, + MRCAL_NSTATE_CALOBJECT_WARP); + + iMeasurement++; + } + } + if(gradient_sparse_meta.pool != NULL) + splined_intrinsics_grad_irun++; + } + } + + // Handle all the point observations. This is VERY similar to the + // board-observation loop above. Please consolidate + for(int i_observation_point = 0; + i_observation_point < ctx->Nobservations_point; + i_observation_point++) + { + const mrcal_observation_point_t* observation = &ctx->observations_point[i_observation_point]; + + const int icam_intrinsics = observation->icam.intrinsics; + const int icam_extrinsics = observation->icam.extrinsics; + const int i_point = observation->i_point; + const bool use_position_from_state = + ctx->problem_selections.do_optimize_frames && + i_point < ctx->Npoints - ctx->Npoints_fixed; + + const mrcal_point3_t* qx_qy_w__observed = &ctx->observations_point_pool[i_observation_point]; + double weight = qx_qy_w__observed->z; + + if(weight <= 0.0) + { + // Outlier. Cost = 0. Jacobians are 0 too, but I must preserve the + // structure + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + // invalid if icam_extrinsics < 0, but unused in that case + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + const int i_var_point = + mrcal_state_index_points(i_point, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + // I have my two measurements (dx, dy). I propagate their + // gradient and store them + for( int i_xy=0; i_xy<2; i_xy++ ) + { + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = 0; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + // fx,fy. x depends on fx only. y depends on fy only + STORE_JACOBIAN( i_var_intrinsics + i_xy, 0 ); + + // cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, 0); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if( (ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions) && + ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC ) + { + // sparse gradient. This is an outlier, so it doesn't + // matter which points I say I depend on, as long as I + // pick the right number, and says that j=0. I pick the + // control points at the start because why not + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config; + int runlen = config->order+1; + for(int i=0; iNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, 0); + } + + if(icam_extrinsics >= 0 && ctx->problem_selections.do_optimize_extrinsics ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, 0,0,0 ); + STORE_JACOBIAN3( i_var_camera_rt + 3, 0,0,0 ); + } + + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, 0,0,0 ); + + iMeasurement++; + } + + // TEMPORARY TWEAK: disable range normalization + // I will re-add this later +#if 0 + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = 0; + if(icam_extrinsics >= 0 && ctx->problem_selections.do_optimize_extrinsics ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, 0,0,0 ); + STORE_JACOBIAN3( i_var_camera_rt + 3, 0,0,0 ); + } + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, 0,0,0 ); + iMeasurement++; +#endif + + continue; + } + + + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + // invalid if icam_extrinsics < 0, but unused in that case + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + const int i_var_point = + mrcal_state_index_points(i_point, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + mrcal_point3_t point_ref; + if(use_position_from_state) + unpack_solver_state_point_one(&point_ref, &packed_state[i_var_point]); + else + point_ref = ctx->points[i_point]; + + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + + // WARNING: "compute size(dq_dintrinsics_pool_double) correctly and maybe bounds-check" + std::vector dq_dintrinsics_pool_double(Ngradients); + // used for LENSMODEL_SPLINED_STEREOGRAPHIC only, but getting rid of + // this in other cases isn't worth the trouble + int dq_dintrinsics_pool_int [1]; + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; + + mrcal_point3_t dq_drcamera[2]; + mrcal_point3_t dq_dtcamera[2]; + mrcal_point3_t dq_dpoint [2]; + + // The array reference [-3] is intended, but the compiler throws a + // warning. I silence it here +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" + mrcal_point2_t q_hypothesis; + project(&q_hypothesis, + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_int : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + ctx->problem_selections.do_optimize_extrinsics ? + dq_drcamera : NULL, + ctx->problem_selections.do_optimize_extrinsics ? + dq_dtcamera : NULL, + NULL, // frame rotation. I only have a point position + use_position_from_state ? dq_dpoint : NULL, + NULL, + + // input + intrinsics_all[icam_intrinsics].data(), + &camera_rt[icam_extrinsics], + + // I only have the point position, so the 'rt' memory + // points 3 back. The fake "r" here will not be + // referenced + (mrcal_pose_t*)(&point_ref.xyz[-3]), + NULL, + + icam_extrinsics < 0, + &ctx->lensmodel, &ctx->precomputed, + 0,0,0); +#pragma GCC diagnostic pop + + // I have my two measurements (dx, dy). I propagate their + // gradient and store them + for( int i_xy=0; i_xy<2; i_xy++ ) + { + const double err = (q_hypothesis.xy[i_xy] - qx_qy_w__observed->xyz[i_xy])*weight; + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = err; + norm2_error += err*err; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + // fx,fy. x depends on fx only. y depends on fy only + STORE_JACOBIAN( i_var_intrinsics + i_xy, + dq_dfxy[i_xy] * + weight * SCALE_INTRINSICS_FOCAL_LENGTH ); + + // cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, + weight * SCALE_INTRINSICS_CENTER_PIXEL ); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if(gradient_sparse_meta.pool != NULL) + { + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3]) + const int ivar0 = dq_dintrinsics_pool_int[0] - + ( ctx->problem_selections.do_optimize_intrinsics_core ? 0 : 4 ); + + const int len = gradient_sparse_meta.run_side_length; + const double* ABCDx = &gradient_sparse_meta.pool[0]; + const double* ABCDy = &gradient_sparse_meta.pool[len]; + + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + const double* fxy = &intrinsics_all[icam_intrinsics][0]; + + for(int iy=0; iyNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, + dq_dintrinsics_nocore[i_xy*(ctx->Nintrinsics-Ncore) + + i] * + weight * SCALE_DISTORTION ); + } + } + + if( ctx->problem_selections.do_optimize_extrinsics ) + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, + dq_drcamera[i_xy].xyz[0] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_xy].xyz[1] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_xy].xyz[2] * + weight * SCALE_ROTATION_CAMERA); + STORE_JACOBIAN3( i_var_camera_rt + 3, + dq_dtcamera[i_xy].xyz[0] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_xy].xyz[1] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_xy].xyz[2] * + weight * SCALE_TRANSLATION_CAMERA); + } + + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, + dq_dpoint[i_xy].xyz[0] * + weight * SCALE_POSITION_POINT, + dq_dpoint[i_xy].xyz[1] * + weight * SCALE_POSITION_POINT, + dq_dpoint[i_xy].xyz[2] * + weight * SCALE_POSITION_POINT); + + iMeasurement++; + } + + + // TEMPORARY TWEAK: disable range normalization + // I will re-add this later +#if 0 + // Now the range normalization (make sure the range isn't + // aphysically high or aphysically low) + if(icam_extrinsics < 0) + { + double distsq = + point_ref.x*point_ref.x + + point_ref.y*point_ref.y + + point_ref.z*point_ref.z; + double penalty, dpenalty_ddistsq; + if(model_supports_projection_behind_camera(&ctx->lensmodel) || + point_ref.z > 0.0) + penalty_range_normalization(&penalty, &dpenalty_ddistsq, distsq, ctx,weight); + else + { + penalty_range_normalization(&penalty, &dpenalty_ddistsq, -distsq, ctx,weight); + dpenalty_ddistsq *= -1.; + } + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = penalty; + norm2_error += penalty*penalty; + + if( use_position_from_state ) + { + double scale = 2.0 * dpenalty_ddistsq * SCALE_POSITION_POINT; + STORE_JACOBIAN3( i_var_point, + scale*point_ref.x, + scale*point_ref.y, + scale*point_ref.z ); + } + + iMeasurement++; + } + else + { + // I need to transform the point. I already computed + // this stuff in project()... + double Rc[3*3]; + double d_Rc_rc[9*3]; + + mrcal_R_from_r(Rc, + d_Rc_rc, + camera_rt[icam_extrinsics].r.xyz); + + mrcal_point3_t pcam; + mul_vec3_gen33t_vout(point_ref.xyz, Rc, pcam.xyz); + add_vec(3, pcam.xyz, camera_rt[icam_extrinsics].t.xyz); + + double distsq = + pcam.x*pcam.x + + pcam.y*pcam.y + + pcam.z*pcam.z; + double penalty, dpenalty_ddistsq; + if(model_supports_projection_behind_camera(&ctx->lensmodel) || + pcam.z > 0.0) + penalty_range_normalization(&penalty, &dpenalty_ddistsq, distsq, ctx,weight); + else + { + penalty_range_normalization(&penalty, &dpenalty_ddistsq, -distsq, ctx,weight); + dpenalty_ddistsq *= -1.; + } + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = penalty; + norm2_error += penalty*penalty; + + if( ctx->problem_selections.do_optimize_extrinsics ) + { + // pcam.x = Rc[row0]*point*SCALE + tc + // d(pcam.x)/dr = d(Rc[row0])/drc*point*SCALE + // d(Rc[row0])/drc is 3x3 matrix at &d_Rc_rc[0] + double d_ptcamx_dr[3]; + double d_ptcamy_dr[3]; + double d_ptcamz_dr[3]; + mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*0], d_ptcamx_dr ); + mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*1], d_ptcamy_dr ); + mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*2], d_ptcamz_dr ); + + STORE_JACOBIAN3( i_var_camera_rt + 0, + SCALE_ROTATION_CAMERA* + 2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[0] + + pcam.y*d_ptcamy_dr[0] + + pcam.z*d_ptcamz_dr[0] ), + SCALE_ROTATION_CAMERA* + 2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[1] + + pcam.y*d_ptcamy_dr[1] + + pcam.z*d_ptcamz_dr[1] ), + SCALE_ROTATION_CAMERA* + 2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[2] + + pcam.y*d_ptcamy_dr[2] + + pcam.z*d_ptcamz_dr[2] ) ); + STORE_JACOBIAN3( i_var_camera_rt + 3, + SCALE_TRANSLATION_CAMERA* + 2.0*dpenalty_ddistsq*pcam.x, + SCALE_TRANSLATION_CAMERA* + 2.0*dpenalty_ddistsq*pcam.y, + SCALE_TRANSLATION_CAMERA* + 2.0*dpenalty_ddistsq*pcam.z ); + } + + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, + SCALE_POSITION_POINT* + 2.0*dpenalty_ddistsq*(pcam.x*Rc[0] + pcam.y*Rc[3] + pcam.z*Rc[6]), + SCALE_POSITION_POINT* + 2.0*dpenalty_ddistsq*(pcam.x*Rc[1] + pcam.y*Rc[4] + pcam.z*Rc[7]), + SCALE_POSITION_POINT* + 2.0*dpenalty_ddistsq*(pcam.x*Rc[2] + pcam.y*Rc[5] + pcam.z*Rc[8]) ); + iMeasurement++; + } +#endif + } + + // Handle all the triangulated point observations. This is VERY similar to + // the board-observation loop above. Please consolidate + if( ctx->observations_point_triangulated != NULL && + ctx->Nobservations_point_triangulated ) + { + if( ! (!ctx->problem_selections.do_optimize_intrinsics_core && + !ctx->problem_selections.do_optimize_intrinsics_distortions && + ctx->problem_selections.do_optimize_extrinsics ) ) + { + // Shouldn't get here. Have a check with an error message in + // mrcal_optimize() and mrcal_optimizer_callback() + assert(0); + } + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: mrcal_observation_point_t.px is the observation vector in camera coords. No outliers. No intrinsics" +// #warning "triangulated-solve: make weights work somehow. This is tied to outliers" +#endif + + for(int i0 = 0; + i0 < ctx->Nobservations_point_triangulated; + i0++) + { + const mrcal_observation_point_triangulated_t* pt0 = + &ctx->observations_point_triangulated[i0]; + if(pt0->last_in_set) + continue; + + if(!pt0->outlier) + { + // For better code efficiency I compute the triangulation in the + // camera-1 coord system. This is because this code looks like + // + // for(point0) + // { + // compute_stuff_for_point0; + // for(point1) + // { + // compute_stuff_for_point1; + // compute stuff based on products of point0 and point1; + // } + // } + // + // Doing the triangulation in the frame of point1 allows me to do + // more stuff in the outer compute_stuff_for_point0 computation, and + // less in the inner compute_stuff_for_point1 computation + + // I need t10. I'm looking at a composition Rt_10 = Rt_1r*Rt_r0 = + // (R_1r,t_1r)*(R_r0,t_r0) = (R_10, R_1r*t_r0 + t_1r) -> t_10 = + // R_1r*t_r0 + t_1r = transform(Rt_1r, t_r0) + // + // I don't actually have t_r0: I have t_0r, so I need to compute an + // inversion. y = R x + t -> x = Rinv y - Rinv t -> tinv = -Rinv t + // t_r0 = -R_r0 t_0r + + const mrcal_point3_t* v0 = &pt0->px; + + const mrcal_point3_t* t_r0; + mrcal_point3_t _t_r0; + double dnegt_r0__dr_0r[3][3]; + double dnegt_r0__dt_0r[3][3]; + + const mrcal_point3_t* v0_ref; + mrcal_point3_t _v0_ref; + double dv0_ref__dr_0r[3][3]; + + const int icam_extrinsics0 = pt0->icam.extrinsics; + if( icam_extrinsics0 >= 0 ) + { + const mrcal_pose_t* rt_0r = &camera_rt[icam_extrinsics0]; + const double* r_0r = &rt_0r->r.xyz[0]; + const double* t_0r = &rt_0r->t.xyz[0]; + + t_r0 = &_t_r0; + v0_ref = &_v0_ref; + + mrcal_rotate_point_r_inverted(_t_r0.xyz, + &dnegt_r0__dr_0r[0][0], + &dnegt_r0__dt_0r[0][0], + + r_0r, t_0r); + for(int i=0; i<3; i++) + _t_r0.xyz[i] *= -1.; + + mrcal_rotate_point_r_inverted(_v0_ref.xyz, + &dv0_ref__dr_0r[0][0], + NULL, + + r_0r, v0->xyz); + } + else + { + t_r0 = NULL; + v0_ref = v0; + } + + + int i1 = i0+1; + + while(true) + { + const mrcal_observation_point_triangulated_t* pt1 = + &ctx->observations_point_triangulated[i1]; + + if(!pt1->outlier) + { + const mrcal_point3_t* v1 = &pt1->px; + + const mrcal_point3_t* t_10; + mrcal_point3_t _t_10; + const mrcal_point3_t* v0_cam1; + mrcal_point3_t _v0_cam1; + + double dt_10__drt_1r [3][6]; + double dt_10__dt_r0 [3][3]; + double dv0_cam1__dr_1r [3][3]; + double dv0_cam1__dv0_ref[3][3]; + + const int icam_extrinsics1 = pt1->icam.extrinsics; + if( icam_extrinsics1 >= 0 ) + { + const mrcal_pose_t* rt_1r = &camera_rt[icam_extrinsics1]; + + v0_cam1 = &_v0_cam1; + + + if( icam_extrinsics0 >= 0 ) + { + t_10 = &_t_10; + mrcal_transform_point_rt( &_t_10.xyz[0], + &dt_10__drt_1r[0][0], + &dt_10__dt_r0 [0][0], + &rt_1r->r.xyz[0], &t_r0->xyz[0] ); + } + else + { + // t_r0 = 0 -> + // + // t_10 = R_1r*t_r0 + t_1r = + // = R_1r*0 + t_1r = + // = t_1r + t_10 = &rt_1r->t; + } + + mrcal_rotate_point_r( &_v0_cam1.xyz[0], + &dv0_cam1__dr_1r [0][0], + &dv0_cam1__dv0_ref[0][0], + &rt_1r->r.xyz[0], &v0_ref->xyz[0] ); + } + else + { + // rt_1r = 0 -> + // + // t_10 = R_1r*t_r0 + t_1r = + // = t_r0 + t_10 = t_r0; + // At most one camera can sit at the reference. So if I'm + // here, I know that t_r0 != NULL and thus t_10 != NULL + + v0_cam1 = v0_ref; + } + + mrcal_point3_t derr__dv0_cam1; + mrcal_point3_t derr__dt_10; + + double err = + _mrcal_triangulated_error(&derr__dv0_cam1, &derr__dt_10, + v1, v0_cam1, t_10); + + + x[iMeasurement] = err; + norm2_error += err*err; + + if(Jt) + { + Jrowptr[iMeasurement] = iJacobian; + + // Now I propagate gradients. Dependency graph: + // + // derr__dv0_cam1 + // dv0_cam1__dr_1r + // dv0_cam1__dv0_ref + // dv0_ref__dr_0r + // + // derr__dt_10 + // dt_10__drt_1r + // dt_10__dt_r0 + // dnegt_r0__dr_0r + // dnegt_r0__dt_0r + // + // So + // + // derr/dr_0r = + // derr/dv0_cam1 dv0_cam1/dv0_ref dv0_ref/dr_0r + + // derr/dt_10 dt_10/dt_r0 dt_r0/dr_0r + // + // derr/dt_0r = + // derr/dt_10 dt_10/dt_r0 dt_r0/dt_0r + // + // derr/dr_1r = + // derr/dv0_cam1 dv0_cam1/dr_1r + + // derr/dt_10 dt_10/dr_1r + // + // derr/dt_1r = + // derr/dt_10 dt_10/dt_1r + if( icam_extrinsics0 >= 0 ) + { + const int i_var_camera_rt0 = + mrcal_state_index_extrinsics(icam_extrinsics0, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + double* out; + + out = &Jval[iJacobian]; + double* derr__dt_r0; + double _derr__dt_r0[3]; + + if( icam_extrinsics1 >= 0 ) + { + derr__dt_r0 = _derr__dt_r0; + mul_vec3t_gen33(derr__dt_r0, derr__dt_10.xyz, &dt_10__dt_r0[0][0], 1, ); + + double temp[3]; + mul_vec3t_gen33(temp, derr__dv0_cam1.xyz, &dv0_cam1__dv0_ref[0][0], 1, ); + mul_vec3t_gen33(out, temp, &dv0_ref__dr_0r[0][0], 1, ); + } + else + { + // camera1 is at the reference, so I don't have + // dt_10__dt_r0 and dv0_cam1__dv0_ref explicitly + // stored. + // + // t_10 = t_r0 --> dt_10__dt_r0 = I + // v0_cam1 = v0_ref --> dv0_cam1__dv0_ref = I + derr__dt_r0 = derr__dt_10.xyz; + mul_vec3t_gen33(out, derr__dv0_cam1.xyz, &dv0_ref__dr_0r[0][0], 1, ); + } + + + mul_vec3t_gen33(out, derr__dt_r0, &dnegt_r0__dr_0r[0][0], -1, _accum); + + SCALE_JACOBIAN_N( i_var_camera_rt0 + 0, + SCALE_ROTATION_CAMERA, + 3 ); + + + out = &Jval[iJacobian]; + mul_vec3t_gen33(out, derr__dt_r0, &dnegt_r0__dt_0r[0][0], -1, ); + + SCALE_JACOBIAN_N( i_var_camera_rt0 + 3, + SCALE_TRANSLATION_CAMERA, + 3 ); + } + if( icam_extrinsics1 >= 0 ) + { + const int i_var_camera_rt1 = + mrcal_state_index_extrinsics(icam_extrinsics1, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + double* out; + + out = &Jval[iJacobian]; + + // derr/dr_1r = + // derr/dv0_cam1 dv0_cam1/dr_1r + + // derr/dt_10 dt_10/dr_1r + // + // derr/dt_1r = + // derr/dt_10 dt_10/dt_1r + mul_vec3t_gen33(out, + derr__dv0_cam1.xyz, + &dv0_cam1__dr_1r[0][0], + 1, + ); + + if( icam_extrinsics0 >= 0 ) + { + mul_genNM_genML_accum(out, 3,1, + 1,3,3, + derr__dt_10.xyz, 3,1, + &dt_10__drt_1r[0][0], 6, 1, + 1); + SCALE_JACOBIAN_N( i_var_camera_rt1 + 0, + SCALE_ROTATION_CAMERA, + 3 ); + + out = &Jval[iJacobian]; + mul_genNM_genML(out, 3,1, + 1,3,3, + derr__dt_10.xyz, 3,1, + &dt_10__drt_1r[0][3], 6, 1, + 1); + + SCALE_JACOBIAN_N( i_var_camera_rt1 + 3, + SCALE_TRANSLATION_CAMERA, + 3 ); + } + else + { + // camera0 is at the reference. dt_10__drt_1r is not + // given explicitly + // + // t_10 = t_1r -> + // dt_10__dr_1r = 0 + // dt_10__dt_1r = I + // So + // + // derr/dr_1r = derr/dv0_cam1 dv0_cam1/dr_1r + // derr/dt_1r = derr/dt_10 + SCALE_JACOBIAN_N( i_var_camera_rt1 + 0, + SCALE_ROTATION_CAMERA, + 3 ); + + out = &Jval[iJacobian]; + + for(int i=0; i<3; i++) + out[i] = derr__dt_10.xyz[i]; + + SCALE_JACOBIAN_N( i_var_camera_rt1 + 3, + SCALE_TRANSLATION_CAMERA, + 3 ); + } + } + } + else + { + // Don't need the Jacobian. I just move iJacobian as needed + if( icam_extrinsics0 >= 0 ) + iJacobian += 6; + if( icam_extrinsics1 >= 0 ) + iJacobian += 6; + } + } + else + { + // pt1 is an outlier + const double err = 0.0; + + const int icam_extrinsics1 = pt1->icam.extrinsics; + + x[iMeasurement] = err; + norm2_error += err*err; + + if(Jt) + { + Jrowptr[iMeasurement] = iJacobian; + + if( icam_extrinsics0 >= 0 ) + { + const int i_var_camera_rt0 = + mrcal_state_index_extrinsics(icam_extrinsics0, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + STORE_JACOBIAN_N( i_var_camera_rt0 + 0, + (double*)NULL, 0.0, + 3 ); + STORE_JACOBIAN_N( i_var_camera_rt0 + 3, + (double*)NULL, 0.0, + 3 ); + } + if( icam_extrinsics1 >= 0 ) + { + const int i_var_camera_rt1 = + mrcal_state_index_extrinsics(icam_extrinsics1, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + STORE_JACOBIAN_N( i_var_camera_rt1 + 0, + (double*)NULL, 0.0, + 3 ); + STORE_JACOBIAN_N( i_var_camera_rt1 + 3, + (double*)NULL, 0.0, + 3 ); + } + } + else + { + // Don't need the Jacobian. I just move iJacobian as needed + if( icam_extrinsics0 >= 0 ) + iJacobian += 6; + if( icam_extrinsics1 >= 0 ) + iJacobian += 6; + } + } + + iMeasurement++; + + if(pt1->last_in_set) + break; + i1++; + } + } + else + { + // pt0 is an outlier. I loop through all the pairwise + // observations, but I ignore ALL of them + const double err = 0.0; + + const int icam_extrinsics0 = pt0->icam.extrinsics; + int i1 = i0+1; + + while(true) + { + const mrcal_observation_point_triangulated_t* pt1 = + &ctx->observations_point_triangulated[i1]; + + const int icam_extrinsics1 = pt1->icam.extrinsics; + + x[iMeasurement] = err; + norm2_error += err*err; + + if(Jt) + { + Jrowptr[iMeasurement] = iJacobian; + + if( icam_extrinsics0 >= 0 ) + { + const int i_var_camera_rt0 = + mrcal_state_index_extrinsics(icam_extrinsics0, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + STORE_JACOBIAN_N( i_var_camera_rt0 + 0, + (double*)NULL, 0.0, + 3 ); + STORE_JACOBIAN_N( i_var_camera_rt0 + 3, + (double*)NULL, 0.0, + 3 ); + } + if( icam_extrinsics1 >= 0 ) + { + const int i_var_camera_rt1 = + mrcal_state_index_extrinsics(icam_extrinsics1, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + STORE_JACOBIAN_N( i_var_camera_rt1 + 0, + (double*)NULL, 0.0, + 3 ); + STORE_JACOBIAN_N( i_var_camera_rt1 + 3, + (double*)NULL, 0.0, + 3 ); + } + } + else + { + // Don't need the Jacobian. I just move iJacobian as needed + if( icam_extrinsics0 >= 0 ) + iJacobian += 6; + if( icam_extrinsics1 >= 0 ) + iJacobian += 6; + } + + iMeasurement++; + + if(pt1->last_in_set) + break; + i1++; + } + } + } + } + + ///////////////// Regularization + { + const bool dump_regularizaton_details = false; + + // I want the total regularization cost to be low relative to the + // other contributions to the cost. And I want each set of + // regularization terms to weigh roughly the same. Let's say I want + // regularization to account for ~ .5% of the other error + // contributions: + // + // Nmeasurements_rest*normal_pixel_error_sq * 0.005/Nregularization_types = + // Nmeasurements_regularization_distortion *normal_regularization_distortion_error_sq = + // Nmeasurements_regularization_centerpixel*normal_regularization_centerpixel_error_sq = + // Nmeasurements_regularization_unity_cam01*normal_regularization_unity_cam01_error_sq + // + // normal_regularization_distortion_error_sq = (scale*normal_distortion_offset )^2 + // normal_regularization_centerpixel_error_sq = (scale*normal_centerpixel_value )^2 + // normal_regularization_unity_cam01_error_sq = (scale*normal_unity_cam01_value )^2 + // + // Regularization introduces a bias to the solution. The + // test-projection-uncertainty test measures it, and barfs if it is too + // high. The constants should be adjusted if that test fails. + int Nmeasurements_regularization_distortion = 0; + int Nmeasurements_regularization_centerpixel = 0; + int Nmeasurements_regularization_unity_cam01 = 0; + + if(ctx->problem_selections.do_apply_regularization) + { + if(ctx->problem_selections.do_optimize_intrinsics_distortions) + Nmeasurements_regularization_distortion = + ctx->Ncameras_intrinsics*(ctx->Nintrinsics-Ncore); + if(ctx->problem_selections.do_optimize_intrinsics_core) + Nmeasurements_regularization_centerpixel = + ctx->Ncameras_intrinsics*2; + } + if(ctx->problem_selections.do_apply_regularization_unity_cam01 && + ctx->problem_selections.do_optimize_extrinsics && + ctx->Ncameras_extrinsics > 0) + { + Nmeasurements_regularization_unity_cam01 = 1; + } + + int Nmeasurements_nonregularization = + ctx->Nmeasurements - + (Nmeasurements_regularization_distortion + + Nmeasurements_regularization_centerpixel + + Nmeasurements_regularization_unity_cam01); + + double normal_pixel_error = 1.0; + double expected_total_pixel_error_sq = + (double)Nmeasurements_nonregularization * + normal_pixel_error * + normal_pixel_error; + if(dump_regularizaton_details) + MSG("expected_total_pixel_error_sq: %f", expected_total_pixel_error_sq); + + // This is set to 2 to match what mrcal 2.4 does, to keep the behavior + // consistent. The exact value doesn't matter. In a previous commit (the + // merge 5c3bdd2b) this was changed to 3, and I'm about to revert it + // back to 2 (2024/07) + const int Nregularization_types = 2; + + if(ctx->problem_selections.do_apply_regularization && + (ctx->problem_selections.do_optimize_intrinsics_distortions || + ctx->problem_selections.do_optimize_intrinsics_core)) + { + double scale_regularization_distortion = 0.0; + double scale_regularization_centerpixel = 0.0; + + // compute scales + { + if(ctx->problem_selections.do_optimize_intrinsics_distortions) + { + // I need to control this better, but this is sufficient for + // now. I need 2.0e-1 for splined models to effectively + // eliminate the curl in the splined model vector field. For + // other models I use 2.0 because that's what I had for a long + // time, and I don't want to change it to not break anything + double normal_distortion_value = + ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC ? + 2.0e-1 : + 2.0; + + double expected_regularization_distortion_error_sq_noscale = + (double)Nmeasurements_regularization_distortion * + normal_distortion_value * + normal_distortion_value; + + double scale_sq = + expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_distortion_error_sq_noscale; + + if(dump_regularizaton_details) + MSG("expected_regularization_distortion_error_sq: %f", expected_regularization_distortion_error_sq_noscale*scale_sq); + + scale_regularization_distortion = sqrt(scale_sq); + } + + if(modelHasCore_fxfycxcy(&ctx->lensmodel) && + ctx->problem_selections.do_optimize_intrinsics_core) + { + double normal_centerpixel_offset = 500.0; + + double expected_regularization_centerpixel_error_sq_noscale = + (double)Nmeasurements_regularization_centerpixel * + normal_centerpixel_offset * + normal_centerpixel_offset; + + double scale_sq = + expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_centerpixel_error_sq_noscale; + + if(dump_regularizaton_details) + MSG("expected_regularization_centerpixel_error_sq: %f", expected_regularization_centerpixel_error_sq_noscale*scale_sq); + + scale_regularization_centerpixel = sqrt(scale_sq); + } + } + + // compute and store regularization terms + { + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + for(int icam_intrinsics=0; icam_intrinsicsNcameras_intrinsics; icam_intrinsics++) + { + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + if(ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + // Splined model regularization. I do directional L2 + // regularization. At each knot I penalize contributions in + // the tangential direction much more than in the radial + // direction. Otherwise noise in the data produces lots of + // curl in the vector field. This isn't wrong, but it's much + // nicer if "right" in the camera coordinate system + // corresponds to "right" in pixel space + const int Nx = ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx; + const int Ny = ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny; + + for(int iy=0; iyNintrinsics-Ncore; j++) + { + // This maybe should live elsewhere, but I put it here + // for now. Various distortion coefficients have + // different meanings, and should be regularized in + // different ways. Specific logic follows + double scale = scale_regularization_distortion; + + if( MRCAL_LENSMODEL_IS_OPENCV(ctx->lensmodel.type) && + ctx->lensmodel.type >= MRCAL_LENSMODEL_OPENCV8 && + 5 <= j && j <= 7 ) + { + // The radial distortion in opencv is x_distorted = + // x*scale where r2 = norm2(xy - xyc) and + // + // scale = (1 + k0 r2 + k1 r4 + k4 r6)/(1 + k5 r2 + k6 r4 + k7 r6) + // + // Note that k2,k3 are tangential (NOT radial) + // distortion components. Note that the r6 factor in + // the numerator is only present for + // >=MRCAL_LENSMODEL_OPENCV5. Note that the denominator + // is only present for >= MRCAL_LENSMODEL_OPENCV8. The + // danger with a rational model is that it's + // possible to get into a situation where scale ~ + // 0/0 ~ 1. This would have very poorly behaved + // derivatives. If all the rational coefficients are + // ~0, then the denominator is always ~1, and this + // problematic case can't happen. I favor that by + // regularizing the coefficients in the denominator + // more strongly + scale *= 5.; + } + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + double err = scale*intrinsics_all[icam_intrinsics][j+Ncore]; + x[iMeasurement] = err; + norm2_error += err*err; + + STORE_JACOBIAN( i_var_intrinsics + Ncore_state + j, + scale * SCALE_DISTORTION ); + + iMeasurement++; + if(dump_regularizaton_details) + MSG("regularization distortion: %g; norm2: %g", err, err*err); + + } + } + } + + if( modelHasCore_fxfycxcy(&ctx->lensmodel) && + ctx->problem_selections.do_optimize_intrinsics_core ) + for(int icam_intrinsics=0; icam_intrinsicsNcameras_intrinsics; icam_intrinsics++) + { + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + // And another regularization term: optical center should be + // near the middle. This breaks the symmetry between moving the + // center pixel coords and pitching/yawing the camera. + double cx_target = 0.5 * (double)(ctx->imagersizes[icam_intrinsics*2 + 0] - 1); + double cy_target = 0.5 * (double)(ctx->imagersizes[icam_intrinsics*2 + 1] - 1); + + double err; + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + err = scale_regularization_centerpixel * + (intrinsics_all[icam_intrinsics][2] - cx_target); + x[iMeasurement] = err; + norm2_error += err*err; + STORE_JACOBIAN( i_var_intrinsics + 2, + scale_regularization_centerpixel * SCALE_INTRINSICS_CENTER_PIXEL ); + iMeasurement++; + if(dump_regularizaton_details) + MSG("regularization center pixel off-center: %g; norm2: %g", err, err*err); + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + err = scale_regularization_centerpixel * + (intrinsics_all[icam_intrinsics][3] - cy_target); + x[iMeasurement] = err; + norm2_error += err*err; + STORE_JACOBIAN( i_var_intrinsics + 3, + scale_regularization_centerpixel * SCALE_INTRINSICS_CENTER_PIXEL ); + iMeasurement++; + if(dump_regularizaton_details) + MSG("regularization center pixel off-center: %g; norm2: %g", err, err*err); + } + } + } + + + if(ctx->problem_selections.do_apply_regularization_unity_cam01 && + ctx->problem_selections.do_optimize_extrinsics && + ctx->Ncameras_extrinsics > 0) + { + double scale_regularization_unity_cam01 = 0.0; + + // compute scales + { +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: better unity_cam01 scale" +#endif + double normal_unity_cam01_value = 1.0; + + double expected_regularization_unity_cam01_error_sq_noscale = + (double)Nmeasurements_regularization_unity_cam01 * + normal_unity_cam01_value * + normal_unity_cam01_value; + + double scale_sq = + expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_unity_cam01_error_sq_noscale; + + if(dump_regularizaton_details) + MSG("expected_regularization_unity_cam01_error_sq: %f", expected_regularization_unity_cam01_error_sq_noscale*scale_sq); + + scale_regularization_unity_cam01 = sqrt(scale_sq); + } + + // compute and store regularization terms + { + // I have the pose for the first camera: rt_0r. The distance + // between the origin of this camera and the origin of the + // reference is t_0r + const mrcal_point3_t* t_0r = &camera_rt[0].t; + + const int i_var_extrinsics = + mrcal_state_index_extrinsics(0, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + double err = + scale_regularization_unity_cam01 * + (norm2_vec(3, t_0r->xyz) - 1.); + x[iMeasurement] = err; + norm2_error += err*err; + + for(int i=0; i<3; i++) + STORE_JACOBIAN( i_var_extrinsics+3 + i, + scale_regularization_unity_cam01 * SCALE_TRANSLATION_CAMERA * + 2.* t_0r->xyz[i]); + + iMeasurement++; + if(dump_regularizaton_details) + MSG("regularization unity_cam01: %g; norm2: %g", err, err*err); + } + } + } + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + if(iMeasurement != ctx->Nmeasurements) + { + MSG("Assertion (iMeasurement == ctx->Nmeasurements) failed: (%d != %d)", + iMeasurement, ctx->Nmeasurements); + assert(0); + } + if(iJacobian != ctx->N_j_nonzero ) + { + MSG("Assertion (iJacobian == ctx->N_j_nonzero ) failed: (%d != %d)", + iJacobian, ctx->N_j_nonzero); + assert(0); + } +} + +bool mrcal_optimizer_callback(// out + + // These output pointers may NOT be NULL, unlike + // their analogues in mrcal_optimize() + + // Shape (Nstate,) + double* b_packed, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_b_packed, + + // Shape (Nmeasurements,) + double* x, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x, + + // output Jacobian. May be NULL if we don't need + // it. This is the unitless Jacobian, used by the + // internal optimization routines + cholmod_sparse* Jt, + + + // in + + // intrinsics is a concatenation of the intrinsics core + // and the distortion params. The specific distortion + // parameters may vary, depending on lensmodel, so + // this is a variable-length structure + const double* intrinsics, // Ncameras_intrinsics * NlensParams + const mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + const mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + const mrcal_point3_t* points, // Npoints of these. In the reference frame + const mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + int Nobservations_board, + int Nobservations_point, + + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + // All the board pixel observations, in order. .x, + // .y are the pixel observations .z is the weight + // of the observation. Most of the weights are + // expected to be 1.0. Less precise observations + // have lower weights. + // + // z<0 indicates that this is an outlier + const mrcal_point3_t* observations_board_pool, + + // Same thing, but for discrete points. Array of shape + // + // ( Nobservations_point,) + const mrcal_point3_t* observations_point_pool, + + const mrcal_lensmodel_t* lensmodel, + const int* imagersizes, // Ncameras_intrinsics*2 of these + + mrcal_problem_selections_t problem_selections, + const mrcal_problem_constants_t* problem_constants, + + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n, + bool verbose) +{ + bool result = false; + + if( ( observations_point_triangulated != NULL && + Nobservations_point_triangulated ) && + ! (!problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions && + problem_selections.do_optimize_extrinsics ) ) + { + MSG("ERROR: We have triangulated points. At this time this is only allowed if we're NOT optimizing intrinsics AND if we ARE optimizing extrinsics."); + return result; + } + + if( Nobservations_board > 0 ) + { + if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL ) + { + MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in."); + return result; + } + } + else + problem_selections.do_optimize_calobject_warp = false; + + if(!modelHasCore_fxfycxcy(lensmodel)) + problem_selections.do_optimize_intrinsics_core = false; + + if(!problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions && + !problem_selections.do_optimize_extrinsics && + !problem_selections.do_optimize_frames && + !problem_selections.do_optimize_calobject_warp) + { + MSG("Not optimizing any of our variables!"); + return result; + } + + + const int Nstate = mrcal_num_states(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel); + if( buffer_size_b_packed != Nstate*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in b_packed has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nstate*(int)sizeof(double),buffer_size_b_packed); + return result; + } + + int Nmeasurements = mrcal_num_measurements(Nobservations_board, + Nobservations_point, + observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, + problem_selections, + lensmodel); + int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + int N_j_nonzero = _mrcal_num_j_nonzero(Nobservations_board, + Nobservations_point, + observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, + observations_board, + observations_point, + problem_selections, + lensmodel); + + if( buffer_size_x != Nmeasurements*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in x has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nmeasurements*(int)sizeof(double),buffer_size_x); + return result; + } + + const int Npoints_fromBoards = + Nobservations_board * + calibration_object_width_n*calibration_object_height_n; + + const callback_context_t ctx = { + .intrinsics = intrinsics, + .extrinsics_fromref = extrinsics_fromref, + .frames_toref = frames_toref, + .points = points, + .calobject_warp = calobject_warp, + .Ncameras_intrinsics = Ncameras_intrinsics, + .Ncameras_extrinsics = Ncameras_extrinsics, + .Nframes = Nframes, + .Npoints = Npoints, + .Npoints_fixed = Npoints_fixed, + .observations_board = observations_board, + .observations_board_pool = observations_board_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, + .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, + .verbose = verbose, + .lensmodel = *lensmodel, + .imagersizes = imagersizes, + .problem_selections = problem_selections, + .problem_constants = problem_constants, + .calibration_object_spacing = calibration_object_spacing, + .calibration_object_width_n = calibration_object_width_n > 0 ? calibration_object_width_n : 0, + .calibration_object_height_n= calibration_object_height_n > 0 ? calibration_object_height_n : 0, + .Nmeasurements = Nmeasurements, + .N_j_nonzero = N_j_nonzero, + .Nintrinsics = Nintrinsics}; + _mrcal_precompute_lensmodel_data((mrcal_projection_precomputed_t*)&ctx.precomputed, lensmodel); + + pack_solver_state(b_packed, + lensmodel, intrinsics, + extrinsics_fromref, + frames_toref, + points, + calobject_warp, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, Npoints-Npoints_fixed, + Nobservations_board, + Nstate); + + optimizer_callback(b_packed, x, Jt, &ctx); + + result = true; + + return result; +} + +mrcal_stats_t +mrcal_optimize( // out + // Each one of these output pointers may be NULL + + // Shape (Nstate,) + double* b_packed_final, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_b_packed_final, + + // Shape (Nmeasurements,) + double* x_final, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x_final, + + // out, in + + // These are a seed on input, solution on output + + // intrinsics is a concatenation of the intrinsics core and the + // distortion params. The specific distortion parameters may + // vary, depending on lensmodel, so this is a variable-length + // structure + double* intrinsics, // Ncameras_intrinsics * NlensParams + mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + mrcal_point3_t* points, // Npoints of these. In the reference frame + mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + int Nobservations_board, + int Nobservations_point, + + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + // All the board pixel observations, in order. + // .x, .y are the pixel observations + // .z is the weight of the observation. Most of the weights are + // expected to be 1.0, which implies that the noise on the + // observation has standard deviation of + // observed_pixel_uncertainty. observed_pixel_uncertainty scales + // inversely with the weight. + // + // z<0 indicates that this is an outlier. This is respected on + // input (even if !do_apply_outlier_rejection). New outliers are + // marked with z<0 on output, so this isn't const + mrcal_point3_t* observations_board_pool, + + // Same thing, but for discrete points. Array of shape + // + // ( Nobservations_point,) + mrcal_point3_t* observations_point_pool, + + const mrcal_lensmodel_t* lensmodel, + const int* imagersizes, // Ncameras_intrinsics*2 of these + mrcal_problem_selections_t problem_selections, + const mrcal_problem_constants_t* problem_constants, + + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n, + bool verbose, + + bool check_gradient) +{ + if( Nobservations_board > 0 ) + { + if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL ) + { + MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in."); + return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; + } + } + else + problem_selections.do_optimize_calobject_warp = false; + + if( ( observations_point_triangulated != NULL && + Nobservations_point_triangulated ) && + ! (!problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions && + problem_selections.do_optimize_extrinsics ) ) + { + MSG("ERROR: We have triangulated points. At this time this is only allowed if we're NOT optimizing intrinsics AND if we ARE optimizing extrinsics."); + // Because the input to the triangulation routines is unprojected + // vectors, and I don't want to unproject as part of the optimization + // callback. And because I must optimize SOMETHING, so if I have fixed + // intrinsics then the extrinsics cannot be fixed + return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: can I loosen this? Optimizing intrinsics and triangulated points together should work" +#endif + } + + if(!modelHasCore_fxfycxcy(lensmodel)) + problem_selections.do_optimize_intrinsics_core = false; + + if(!problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions && + !problem_selections.do_optimize_extrinsics && + !problem_selections.do_optimize_frames && + !problem_selections.do_optimize_calobject_warp) + { + MSG("Warning: Not optimizing any of our variables"); + } + + dogleg_parameters2_t dogleg_parameters; + dogleg_getDefaultParameters(&dogleg_parameters); + dogleg_parameters.dogleg_debug = verbose ? DOGLEG_DEBUG_VNLOG : 0; + + // These were derived empirically, seeking high accuracy, fast convergence + // and without serious concern for performance. I looked only at a single + // frame. Tweak them please + dogleg_parameters.Jt_x_threshold = 0; + dogleg_parameters.update_threshold = 1e-6; + dogleg_parameters.trustregion_threshold = 0; + dogleg_parameters.max_iterations = 300; + // dogleg_parameters.trustregion_decrease_factor = 0.1; + // dogleg_parameters.trustregion_decrease_threshold = 0.15; + // dogleg_parameters.trustregion_increase_factor = 4.0 + // dogleg_parameters.trustregion_increase_threshold = 0.75; + + const int Npoints_fromBoards = + Nobservations_board * + calibration_object_width_n*calibration_object_height_n; + + callback_context_t ctx = { + .intrinsics = intrinsics, + .extrinsics_fromref = extrinsics_fromref, + .frames_toref = frames_toref, + .points = points, + .calobject_warp = calobject_warp, + .Ncameras_intrinsics = Ncameras_intrinsics, + .Ncameras_extrinsics = Ncameras_extrinsics, + .Nframes = Nframes, + .Npoints = Npoints, + .Npoints_fixed = Npoints_fixed, + .observations_board = observations_board, + .observations_board_pool = observations_board_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, + .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, + .verbose = verbose, + .lensmodel = *lensmodel, + .imagersizes = imagersizes, + .problem_selections = problem_selections, + .problem_constants = problem_constants, + .calibration_object_spacing = calibration_object_spacing, + .calibration_object_width_n = calibration_object_width_n > 0 ? calibration_object_width_n : 0, + .calibration_object_height_n= calibration_object_height_n > 0 ? calibration_object_height_n : 0, + .Nmeasurements = mrcal_num_measurements(Nobservations_board, + Nobservations_point, + observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, + problem_selections, + lensmodel), + .N_j_nonzero = _mrcal_num_j_nonzero(Nobservations_board, + Nobservations_point, + observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, + observations_board, + observations_point, + problem_selections, + lensmodel), + .Nintrinsics = mrcal_lensmodel_num_params(lensmodel)}; + _mrcal_precompute_lensmodel_data((mrcal_projection_precomputed_t*)&ctx.precomputed, lensmodel); + + const int Nstate = mrcal_num_states(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel); + + if( b_packed_final != NULL && + buffer_size_b_packed_final != Nstate*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in b_packed_final has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nstate*(int)sizeof(double),buffer_size_b_packed_final); + return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; + } + if( x_final != NULL && + buffer_size_x_final != ctx.Nmeasurements*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in x_final has the wrong size. Needed exactly %d bytes, but got %d bytes", + ctx.Nmeasurements*(int)sizeof(double),buffer_size_x_final); + return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; + } + + + dogleg_solverContext_t* solver_context = NULL; + + if(verbose) + MSG("## Nmeasurements=%d, Nstate=%d", ctx.Nmeasurements, Nstate); + if(ctx.Nmeasurements <= Nstate) + { + MSG("WARNING: problem isn't overdetermined: Nmeasurements=%d, Nstate=%d. Solver may not converge, and if it does, the results aren't reliable. Add more constraints and/or regularization", + ctx.Nmeasurements, Nstate); + } + + // WARNING: is it reasonable to put this on the stack? Can I use + // b_packed_final for this? + std::vector packed_state(Nstate); + pack_solver_state(packed_state.data(), + lensmodel, intrinsics, + extrinsics_fromref, + frames_toref, + points, + calobject_warp, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, Npoints-Npoints_fixed, + Nobservations_board, + Nstate); + + double norm2_error = -1.0; + mrcal_stats_t stats = {.rms_reproj_error__pixels = -1.0 }; + + if( !check_gradient ) + { + stats.Noutliers_board = 0; + stats.Noutliers_triangulated_point = 0; + + const int Nmeasurements_board = + mrcal_num_measurements_boards(Nobservations_board, + calibration_object_width_n, + calibration_object_height_n); + for(int i=0; ibeforeStep, solver_context); +#endif + + } while( problem_selections.do_apply_outlier_rejection && + markOutliers(observations_board_pool, + &stats.Noutliers_board, + &stats.Noutliers_triangulated_point, + observations_board, + Nobservations_board, + Nobservations_point, +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: not const for now. this will become const once the outlier bit moves to the point_triangulated pool" +#endif + (mrcal_observation_point_triangulated_t*)observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + solver_context->beforeStep->x, + extrinsics_fromref, + verbose) && + ([=]{MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again", + stats.Noutliers_board, + Nmeasurements_board, + (double)(stats.Noutliers_board * 100) / (double)Nmeasurements_board); + return true;}())); +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: the above print should deal with triangulated points too" +#endif + + + // Done. I have the final state. I spit it back out + unpack_solver_state( intrinsics, // Ncameras_intrinsics of these + extrinsics_fromref, // Ncameras_extrinsics of these + frames_toref, // Nframes of these + points, // Npoints of these + calobject_warp, + packed_state.data(), + lensmodel, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, Npoints-Npoints_fixed, + Nobservations_board, + Nstate); + + double regularization_ratio_distortion = 0.0; + double regularization_ratio_centerpixel = 0.0; + const int imeas_reg0 = + mrcal_measurement_index_regularization(observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, Nobservations_point, + problem_selections, + lensmodel); + + if(imeas_reg0 >= 0 && verbose) + { + // we have regularization + const double* xreg = &solver_context->beforeStep->x[imeas_reg0]; + + int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0; + + int Nmeasurements_regularization_distortion = 0; + if(problem_selections.do_optimize_intrinsics_distortions) + Nmeasurements_regularization_distortion = + Ncameras_intrinsics*(ctx.Nintrinsics-Ncore); + + int Nmeasurements_regularization_centerpixel = 0; + if(problem_selections.do_optimize_intrinsics_core) + Nmeasurements_regularization_centerpixel = + Ncameras_intrinsics*2; + + double norm2_err_regularization_distortion = 0; + double norm2_err_regularization_centerpixel = 0; + + for(int i=0; i 0.01) + MSG("WARNING: regularization ratio for lens distortion exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", + norm2_err_regularization_distortion, norm2_error, regularization_ratio_distortion); + if(regularization_ratio_centerpixel > 0.01) + MSG("WARNING: regularization ratio for the projection centerpixel exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", + norm2_err_regularization_centerpixel, norm2_error, regularization_ratio_centerpixel); + + double regularization_ratio_unity_cam01 = 0.0; + if(problem_selections.do_apply_regularization_unity_cam01 && + problem_selections.do_optimize_extrinsics && + Ncameras_extrinsics > 0) + { + int Nmeasurements_regularization_unity_cam01 = 1; + double norm2_err_regularization_unity_cam01 = 0; + + for(int i=0; i 0.01) + MSG("WARNING: regularization ratio for unity_cam01 exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", + norm2_err_regularization_unity_cam01, norm2_error, regularization_ratio_unity_cam01); + } + + assert(xreg == &solver_context->beforeStep->x[ctx.Nmeasurements]); + + + // Disable this by default. Splined models have LOTS of + // parameters, and I don't want to print them. Usually. + // + // for(int i=0; ibeforeStep->x[ctx.Nmeasurements - Nmeasurements_regularization + i]; + // MSG("regularization %d: %f (squared: %f)", i, x, x*x); + // } + MSG("reg err ratio (distortion,centerpixel): %.3g %.3g", + regularization_ratio_distortion, + regularization_ratio_centerpixel); + + if(problem_selections.do_apply_regularization_unity_cam01 && + problem_selections.do_optimize_extrinsics && + Ncameras_extrinsics > 0) + { + MSG("reg err ratio (unity_cam01): %.3g", + regularization_ratio_unity_cam01); + } + } + } + else + for(int ivar=0; ivarbeforeStep->p, Nstate*sizeof(double)); + if(x_final) + memcpy(x_final, solver_context->beforeStep->x, ctx.Nmeasurements*sizeof(double)); + + if(solver_context != NULL) + dogleg_freeContext(&solver_context); + + return stats; +} + +bool mrcal_write_cameramodel_file(const char* filename, + const mrcal_cameramodel_t* cameramodel) +{ + bool result = false; + FILE* fp = fopen(filename, "w"); + if(fp == NULL) + { + MSG("Couldn't open('%s')", filename); + return false; + } + + char lensmodel_string[1024]; + if(!mrcal_lensmodel_name(lensmodel_string, sizeof(lensmodel_string), + &cameramodel->lensmodel)) + { + MSG("Couldn't construct lensmodel string. Unconfigured string: '%s'", + mrcal_lensmodel_name_unconfigured(&cameramodel->lensmodel)); + if(fp != NULL) + fclose(fp); + return result; + } + + int Nparams = mrcal_lensmodel_num_params(&cameramodel->lensmodel); + if(Nparams<0) + { + MSG("Couldn't get valid Nparams from lensmodel string '%s'", + lensmodel_string); + if(fp != NULL) + fclose(fp); + return result;; + } + + fprintf(fp, "{\n"); + fprintf(fp, " 'lensmodel': '%s',\n", lensmodel_string); + fprintf(fp, " 'intrinsics': [ "); + for(int i=0; iintrinsics[i]); + fprintf(fp, "],\n"); + fprintf(fp, " 'imagersize': [ %u, %u ],\n", + cameramodel->imagersize[0], + cameramodel->imagersize[1]); + fprintf(fp, " 'extrinsics': [ %f, %f, %f, %f, %f, %f ]\n", + cameramodel->rt_cam_ref[0], + cameramodel->rt_cam_ref[1], + cameramodel->rt_cam_ref[2], + cameramodel->rt_cam_ref[3], + cameramodel->rt_cam_ref[4], + cameramodel->rt_cam_ref[5]); + + fprintf(fp,"}\n"); + result = true; + + if(fp != NULL) + fclose(fp); + return result; +} + + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: fixed points should live in a separate array, instead of at the end of the 'points' array" +#endif diff --git a/wpical/src/main/native/thirdparty/mrcal/src/opencv.c b/wpical/src/main/native/thirdparty/mrcal/src/opencv.c new file mode 100644 index 0000000000..3e751f611b --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/opencv.c @@ -0,0 +1,152 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#include "mrcal.h" + + +// The implementation of _mrcal_project_internal_opencv is based on opencv. The +// sources have been heavily modified, but the opencv logic remains. This +// function is a cut-down cvProjectPoints2Internal() to keep only the +// functionality I want and to use my interfaces. Putting this here allows me to +// drop the C dependency on opencv. Which is a good thing, since opencv dropped +// their C API +// +// from opencv-4.2.0+dfsg/modules/calib3d/src/calibration.cpp +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of + +// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper +// only +void _mrcal_project_internal_opencv( // outputs + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, // may be NULL + double* dq_dintrinsics_nocore, // may be NULL + + // inputs + const mrcal_point3_t* p, + int N, + const double* intrinsics, + int Nintrinsics) +{ + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + + double k[12] = {}; + for(int i=0; i 2 ) + { + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 2] = fx*a1; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 2] = fy*a3; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 3] = fx*a2; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 3] = fy*a1; + if( Nintrinsics-4 > 4 ) + { + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 4] = fx*x*icdist2*r6; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 4] = fy*y*icdist2*r6; + + if( Nintrinsics-4 > 5 ) + { + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 5] = fx*x*cdist*(-icdist2)*icdist2*r2; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 5] = fy*y*cdist*(-icdist2)*icdist2*r2; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 6] = fx*x*cdist*(-icdist2)*icdist2*r4; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 6] = fy*y*cdist*(-icdist2)*icdist2*r4; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 7] = fx*x*cdist*(-icdist2)*icdist2*r6; + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 7] = fy*y*cdist*(-icdist2)*icdist2*r6; + if( Nintrinsics-4 > 8 ) + { + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 8] = fx*r2; //s1 + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 8] = fy*0; //s1 + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 9] = fx*r4; //s2 + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 9] = fy*0; //s2 + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 10] = fx*0; //s3 + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 10] = fy*r2; //s3 + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 11] = fx*0; //s4 + dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 11] = fy*r4; //s4 + } + } + } + } + } + } +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c new file mode 100644 index 0000000000..a0df80b52a --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c @@ -0,0 +1,155 @@ +// The implementation of mrcal_R_from_r is based on opencv. +// The sources have been heavily modified, but the opencv logic remains. +// +// from opencv-4.1.2+dfsg/modules/calib3d/src/calibration.cpp +// +// Copyright (C) 2000-2008, Intel Corporation, all rights reserved. +// Copyright (C) 2009, Willow Garage Inc., all rights reserved. +// Third party copyrights are property of their respective owners. +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of + +// Apparently I need this in MSVC to get constants +#define _USE_MATH_DEFINES + +#include +#include + +#include "poseutils.h" +#include "strides.h" + +void mrcal_R_from_r_full( // outputs + double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // in bytes. <= 0 means "contiguous" + double* J, // (3,3,3) array. Gradient. May be NULL + int J_stride0, // in bytes. <= 0 means "contiguous" + int J_stride1, // in bytes. <= 0 means "contiguous" + int J_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* r, // (3,) vector + int r_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(R, 3,3); + init_stride_3D(J, 3,3,3 ); + init_stride_1D(r, 3 ); + + double norm2r = 0.0; + for(int i=0; i<3; i++) + norm2r += P1(r,i)*P1(r,i); + + if( norm2r < DBL_EPSILON*DBL_EPSILON ) + { + mrcal_identity_R_full(R, R_stride0, R_stride1); + + if( J ) + { + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + for(int k=0; k<3; k++) + P3(J,i,j,k) = 0.; + + P3(J,1,2,0) = -1.; + P3(J,2,0,1) = -1.; + P3(J,0,1,2) = -1.; + + P3(J,2,1,0) = 1.; + P3(J,0,2,1) = 1.; + P3(J,1,0,2) = 1.; + } + return; + } + + double theta = sqrt(norm2r); + + double s = sin(theta); + double c = cos(theta); + double c1 = 1. - c; + double itheta = 1./theta; + + double r_unit[3]; + for(int i=0; i<3; i++) + r_unit[i] = P1(r,i) * itheta; + + // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] + P2(R, 0,0) = c + c1*r_unit[0]*r_unit[0]; + P2(R, 0,1) = c1*r_unit[0]*r_unit[1] - s*r_unit[2]; + P2(R, 0,2) = c1*r_unit[0]*r_unit[2] + s*r_unit[1]; + P2(R, 1,0) = c1*r_unit[0]*r_unit[1] + s*r_unit[2]; + P2(R, 1,1) = c + c1*r_unit[1]*r_unit[1]; + P2(R, 1,2) = c1*r_unit[1]*r_unit[2] - s*r_unit[0]; + P2(R, 2,0) = c1*r_unit[0]*r_unit[2] - s*r_unit[1]; + P2(R, 2,1) = c1*r_unit[1]*r_unit[2] + s*r_unit[0]; + P2(R, 2,2) = c + c1*r_unit[2]*r_unit[2]; + + if( J ) + { + // opencv had some logic with lots of 0s. I unrolled all of the + // loops, and removed all the resulting 0 terms + double a0, a1, a3; + double a2 = itheta * c1; + double a4 = itheta * s; + + a0 = -s *r_unit[0]; + a1 = (s - 2*a2)*r_unit[0]; + a3 = (c - a4)*r_unit[0]; + P3(J,0,0,0) = a0 + a1*r_unit[0]*r_unit[0] + a2*(r_unit[0]+r_unit[0]); + P3(J,0,1,0) = a1*r_unit[0]*r_unit[1] + a2*r_unit[1] - a3*r_unit[2]; + P3(J,0,2,0) = a1*r_unit[0]*r_unit[2] + a2*r_unit[2] + a3*r_unit[1]; + P3(J,1,0,0) = a1*r_unit[0]*r_unit[1] + a2*r_unit[1] + a3*r_unit[2]; + P3(J,1,1,0) = a0 + a1*r_unit[1]*r_unit[1]; + P3(J,1,2,0) = a1*r_unit[1]*r_unit[2] - a3*r_unit[0] - a4; + P3(J,2,0,0) = a1*r_unit[0]*r_unit[2] + a2*r_unit[2] - a3*r_unit[1]; + P3(J,2,1,0) = a1*r_unit[1]*r_unit[2] + a3*r_unit[0] + a4; + P3(J,2,2,0) = a0 + a1*r_unit[2]*r_unit[2]; + + a0 = -s *r_unit[1]; + a1 = (s - 2*a2)*r_unit[1]; + a3 = (c - a4)*r_unit[1]; + P3(J,0,0,1) = a0 + a1*r_unit[0]*r_unit[0]; + P3(J,0,1,1) = a1*r_unit[0]*r_unit[1] + a2*r_unit[0] - a3*r_unit[2]; + P3(J,0,2,1) = a1*r_unit[0]*r_unit[2] + a3*r_unit[1] + a4; + P3(J,1,0,1) = a1*r_unit[0]*r_unit[1] + a2*r_unit[0] + a3*r_unit[2]; + P3(J,1,1,1) = a0 + a1*r_unit[1]*r_unit[1] + a2*(r_unit[1]+r_unit[1]); + P3(J,1,2,1) = a1*r_unit[1]*r_unit[2] + a2*r_unit[2] - a3*r_unit[0]; + P3(J,2,0,1) = a1*r_unit[0]*r_unit[2] - a3*r_unit[1] - a4; + P3(J,2,1,1) = a1*r_unit[1]*r_unit[2] + a2*r_unit[2] + a3*r_unit[0]; + P3(J,2,2,1) = a0 + a1*r_unit[2]*r_unit[2]; + + a0 = -s *r_unit[2]; + a1 = (s - 2*a2)*r_unit[2]; + a3 = (c - a4)*r_unit[2]; + P3(J,0,0,2) = a0 + a1*r_unit[0]*r_unit[0]; + P3(J,0,1,2) = a1*r_unit[0]*r_unit[1] - a3*r_unit[2] - a4; + P3(J,0,2,2) = a1*r_unit[0]*r_unit[2] + a2*r_unit[0] + a3*r_unit[1]; + P3(J,1,0,2) = a1*r_unit[0]*r_unit[1] + a3*r_unit[2] + a4; + P3(J,1,1,2) = a0 + a1*r_unit[1]*r_unit[1]; + P3(J,1,2,2) = a1*r_unit[1]*r_unit[2] + a2*r_unit[1] - a3*r_unit[0]; + P3(J,2,0,2) = a1*r_unit[0]*r_unit[2] + a2*r_unit[0] - a3*r_unit[1]; + P3(J,2,1,2) = a1*r_unit[1]*r_unit[2] + a2*r_unit[1] + a3*r_unit[0]; + P3(J,2,2,2) = a0 + a1*r_unit[2]*r_unit[2] + a2*(r_unit[2]+r_unit[2]); + } +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp new file mode 100644 index 0000000000..deb4670887 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp @@ -0,0 +1,891 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#include "autodiff.hh" +#include "strides.h" + +extern "C" { +#include "poseutils.h" +} + +template +static void +rotate_point_r_core(// output + val_withgrad_t* x_outg, + + // inputs + const val_withgrad_t* rg, + const val_withgrad_t* x_ing, + bool inverted) +{ + // Rodrigues rotation formula: + // xrot = x cos(th) + cross(axis, x) sin(th) + axis axist x (1 - cos(th)) + // + // I have r = axis*th -> th = mag(r) -> + // xrot = x cos(th) + cross(r, x) sin(th)/th + r rt x (1 - cos(th)) / (th*th) + + // if inverted: we have r <- -r, axis <- axis, th <- -th + // + // According to the expression above, this only flips the sign on the + // cross() term + double sign = inverted ? -1.0 : 1.0; + + const val_withgrad_t th2 = + rg[0]*rg[0] + + rg[1]*rg[1] + + rg[2]*rg[2]; + const val_withgrad_t cross[3] = + { + (rg[1]*x_ing[2] - rg[2]*x_ing[1])*sign, + (rg[2]*x_ing[0] - rg[0]*x_ing[2])*sign, + (rg[0]*x_ing[1] - rg[1]*x_ing[0])*sign + }; + const val_withgrad_t inner = + rg[0]*x_ing[0] + + rg[1]*x_ing[1] + + rg[2]*x_ing[2]; + + if(th2.x < 1e-10) + { + // Small rotation. I don't want to divide by 0, so I take the limit + // lim(th->0, xrot) = + // = x + cross(r, x) + r rt x lim(th->0, (1 - cos(th)) / (th*th)) + // = x + cross(r, x) + r rt x lim(th->0, sin(th) / (2*th)) + // = x + cross(r, x) + r rt x/2 + for(int i=0; i<3; i++) + x_outg[i] = + x_ing[i] + + cross[i] + + rg[i]*inner / 2.; + } + else + { + // Non-small rotation. This is the normal path. Note that this path is + // still valid even if th ~ 180deg + + const val_withgrad_t th = th2.sqrt(); + const vec_withgrad_t sc = th.sincos(); + + for(int i=0; i<3; i++) + x_outg[i] = + x_ing[i]*sc.v[1] + + cross[i] * sc.v[0]/th + + rg[i] * inner * (val_withgrad_t(1.) - sc.v[1]) / th2; + } +} + +extern "C" +void mrcal_rotate_point_r_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_r, // (3,3) array. May be NULL + int J_r_stride0, // in bytes. <= 0 means "contiguous" + int J_r_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r, // (3,) array. May be NULL + int r_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_r corresponds + // to the input r + ) +{ + init_stride_1D(x_out, 3); + init_stride_2D(J_r, 3,3); + init_stride_2D(J_x, 3,3); + init_stride_1D(r, 3); + init_stride_1D(x_in, 3); + + if(J_r == NULL && J_x == NULL) + { + vec_withgrad_t<0, 3> rg (r, -1, r_stride0); + vec_withgrad_t<0, 3> x_ing(x_in, -1, x_in_stride0); + vec_withgrad_t<0, 3> x_outg; + rotate_point_r_core<0>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + } + else if(J_r != NULL && J_x == NULL) + { + vec_withgrad_t<3, 3> rg (r, 0, r_stride0); + vec_withgrad_t<3, 3> x_ing(x_in, -1, x_in_stride0); + vec_withgrad_t<3, 3> x_outg; + rotate_point_r_core<3>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_r, 0, 3, 0, J_r_stride0, J_r_stride1); + } + else if(J_r == NULL && J_x != NULL) + { + vec_withgrad_t<3, 3> rg (r, -1, r_stride0); + vec_withgrad_t<3, 3> x_ing(x_in, 0, x_in_stride0); + vec_withgrad_t<3, 3> x_outg; + rotate_point_r_core<3>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_x, 0, 3, 0, J_x_stride0,J_x_stride1); + } + else + { + vec_withgrad_t<6, 3> rg (r, 0, r_stride0); + vec_withgrad_t<6, 3> x_ing(x_in, 3, x_in_stride0); + vec_withgrad_t<6, 3> x_outg; + rotate_point_r_core<6>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_r, 0, 3, 0, J_r_stride0, J_r_stride1); + x_outg.extract_grad (J_x, 3, 3, 0, J_x_stride0, J_x_stride1); + } +} + +extern "C" +void mrcal_transform_point_rt_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_rt, // (3,6) array. May be NULL + int J_rt_stride0, // in bytes. <= 0 means "contiguous" + int J_rt_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt, // (6,) array. May be NULL + int rt_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply the + // transformation in the + // opposite direction. + // J_rt corresponds to + // the input rt + ) +{ + if(!inverted) + { + init_stride_1D(x_out, 3); + init_stride_2D(J_rt, 3,6); + // init_stride_2D(J_x, 3,3 ); + init_stride_1D(rt, 6 ); + // init_stride_1D(x_in, 3 ); + + // to make in-place operations work + double t[3] = { P1(rt, 3), + P1(rt, 4), + P1(rt, 5) }; + // I want rotate(x) + t + // First rotate(x) + mrcal_rotate_point_r_full(x_out, x_out_stride0, + J_rt, J_rt_stride0, J_rt_stride1, + J_x, J_x_stride0, J_x_stride1, + rt, rt_stride0, + x_in, x_in_stride0, false); + + // And now +t. The J_r, J_x gradients are unaffected. J_t is identity + for(int i=0; i<3; i++) + P1(x_out,i) += t[i]; + if(J_rt) + mrcal_identity_R_full(&P2(J_rt,0,3), J_rt_stride0, J_rt_stride1); + } + else + { + // I use the special-case rx_minus_rt() to efficiently rotate both x and + // t by the same r + init_stride_1D(x_out, 3); + init_stride_2D(J_rt, 3,6); + init_stride_2D(J_x, 3,3 ); + init_stride_1D(rt, 6 ); + init_stride_1D(x_in, 3 ); + + if(J_rt == NULL && J_x == NULL) + { + vec_withgrad_t<0, 3> x_minus_t(x_in, -1, x_in_stride0); + x_minus_t -= vec_withgrad_t<0, 3>(&P1(rt,3), -1, rt_stride0); + + vec_withgrad_t<0, 3> rg (&rt[0], -1, rt_stride0); + vec_withgrad_t<0, 3> x_outg; + rotate_point_r_core<0>(x_outg.v, + rg.v, x_minus_t.v, + true); + x_outg.extract_value(x_out, x_out_stride0); + } + else if(J_rt != NULL && J_x == NULL) + { + vec_withgrad_t<6, 3> x_minus_t(x_in, -1, x_in_stride0); + x_minus_t -= vec_withgrad_t<6, 3>(&P1(rt,3), 3, rt_stride0); + + vec_withgrad_t<6, 3> rg (&rt[0], 0, rt_stride0); + vec_withgrad_t<6, 3> x_outg; + rotate_point_r_core<6>(x_outg.v, + rg.v, x_minus_t.v, + true); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_rt, 0, 3, 0, J_rt_stride0, J_rt_stride1); + x_outg.extract_grad (&P2(J_rt,0,3), 3, 3, 0, J_rt_stride0, J_rt_stride1); + } + else if(J_rt == NULL && J_x != NULL) + { + vec_withgrad_t<3, 3> x_minus_t(x_in, 0, x_in_stride0); + x_minus_t -= vec_withgrad_t<3, 3>(&P1(rt,3),-1, rt_stride0); + + vec_withgrad_t<3, 3> rg (&rt[0], -1, rt_stride0); + vec_withgrad_t<3, 3> x_outg; + rotate_point_r_core<3>(x_outg.v, + rg.v, x_minus_t.v, + true); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_x, 0, 3, 0, J_x_stride0, J_x_stride1); + } + else + { + vec_withgrad_t<9, 3> x_minus_t(x_in, 3, x_in_stride0); + x_minus_t -= vec_withgrad_t<9, 3>(&P1(rt,3), 6, rt_stride0); + + vec_withgrad_t<9, 3> rg (&rt[0], 0, rt_stride0); + vec_withgrad_t<9, 3> x_outg; + + + rotate_point_r_core<9>(x_outg.v, + rg.v, x_minus_t.v, + true); + + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_rt, 0, 3, 0, J_rt_stride0, J_rt_stride1); + x_outg.extract_grad (&P2(J_rt,0,3), 6, 3, 0, J_rt_stride0, J_rt_stride1); + x_outg.extract_grad (J_x, 3, 3, 0, J_x_stride0, J_x_stride1); + } + } +} + + +template +static void +compose_r_core(// output + vec_withgrad_t* r, + + // inputs + const vec_withgrad_t* r0, + const vec_withgrad_t* r1) +{ + /* + + Described here: + + Altmann, Simon L. "Hamilton, Rodrigues, and the Quaternion Scandal." + Mathematics Magazine, vol. 62, no. 5, 1989, pp. 291–308 + + Available here: + + https://www.jstor.org/stable/2689481 + + I use Equations (19) and (20) on page 302 of this paper. These equations say + that + + R(angle=gamma, axis=n) = + compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) + + I need to compute r01 = gamma*n from r0=alpha*l, r1=beta*m; and these are + given as solutions to: + + cos(gamma/2) = + cos(alpha/2)*cos(beta/2) - + sin(alpha/2)*sin(beta/2) * inner(l,m) + sin(gamma/2) n = + sin(alpha/2)*cos(beta/2)*l + + cos(alpha/2)*sin(beta/2)*m + + sin(alpha/2)*sin(beta/2) * cross(l,m) + + For nicer notation, I define + + A = alpha/2 + B = beta /2 + C = gamma/2 + + l = r0 /(2A) + m = r1 /(2B) + n = r01/(2C) + + I rewrite: + + cos(C) = + cos(A)*cos(B) - + sin(A)*sin(B) * inner(r0,r1) / 4AB + sin(C) r01 / 2C = + sin(A)*cos(B)*r0 / 2A + + cos(A)*sin(B)*r1 / 2B + + sin(A)*sin(B) * cross(r0,r1) / 4AB + -> + r01 = + C/sin(C) ( sin(A)/A cos(B)*r0 + + sin(B)/B cos(A)*r1 + + sin(A)/A sin(B)/B * cross(r0,r1) / 2 ) + + I compute cos(C) and then get C and sin(C) and r01 from that + + Note that each r = th*axis has equivalent axis*(k*2*pi + th)*axis and + -axis*(k*2*pi - th) for any integer k. + + Let's confirm that rotating A or B by any number full rotations has no + effect on r01. + + We'll have + + alpha += k*2*pi -> A += k*pi + r0 += r0/mag(r0)*k*2*pi + + if k is even: + sin(A), cos(A) stays the same; + r0 / A -> r0 * (1 + k 2pi/mag(r0)) / (A + k pi) + = r0 * (1 + k 2pi/2A) / (A + k pi) + = r0 * (1 + k pi/A) / (A + k pi) + = r0 / A + + So adding an even number of full rotations produces the same exact r01. If + k is odd (adding an odd number of full rotations; should still produce the + same result) we get + + sin(A), cos(A) -> -sin(A),-cos(A) + r0 / A stays the same, as before + + -> cos(C) and sin(C) r01 / 2C = sin(C) n change sign. + + This means that C -> C +- pi. So an odd k switches to a different mode, + but the meaning of the rotation vector r01 does not change + + + What about shifts of r01? + + Let u = sin(A)/A cos(B)*r0 + + sin(B)/B cos(A)*r1 + + sin(A)/A sin(B)/B * cross(r0,r1) / 2 + + and r01 = C/sinC u + + norm2(u) = + = norm2( sA/A cB 2A l + + sB/B cA 2B m + + sA/A sB/B 4AB cross(l,m) / 2 ) + = norm2( sA cB 2 l + + sB cA 2 m + + sA sB 2 cross(l,m) ) + = 4 (sA cB sA cB + + sB cA sB cA + + 2 sA cB sB cA inner(l,m) + + sA sB sA sB norm2(cross(l,m))) + = 4 (sA cB sA cB + + sB cA sB cA + + 2 sA cB sB cA inner(l,m) + + sA sB sA sB (1 - inner^2(l,m)) ) + + I have + + cC = cA cB - sA sB inner(r0,r1) / 4AB + = cA cB - sA sB inner(l,m) + + So inner(l,m) = (cA cB - cC)/(sA sB) + + norm2(u) = + = 4 (sA cB sA cB + + sB cA sB cA + + 2 sA cB sB cA (cA cB - cC)/(sA sB) + + sA sB sA sB (1 - ((cA cB - cC)/(sA sB))^2) ) + = 4 (sA cB sA cB + + sB cA sB cA + + 2 sA cB sB cA (cA cB - cC)/(sA sB) + + sA sB sA sB (1 - (cA cB - cC)^2/(sA sB)^2) ) + = 4 (sA cB sA cB + + sB cA sB cA + + 2 cB cA cA cB + -2 cB cA cC + sA sB sA sB + -cA cB cA cB + -cC cC + + 2 cA cB cC) + = 4 (sA cB sA cB + + sB cA sB cA + + cB cA cA cB + sA sB sA sB + -cC cC) + = 4 (sA sA + + cA cA + + -cC cC) + = 4 (1 - cC cC) + = 4 sC^2 + + r01 = C/sinC u -> + norm2(r01) = C^2/sin^2(C) norm2(u) + = C^2/sin^2(C) 4 sin^2(C) + = 4 C^2 + So mag(r01) = 2C + + This is what we expect. And any C that satisfies the above expressions will + have mag(r01) = 2C + + */ + + const val_withgrad_t A = r0->mag() / 2.; + const val_withgrad_t B = r1->mag() / 2.; + + const val_withgrad_t inner = r0->dot(*r1); + const vec_withgrad_t cross = r0->cross(*r1); + + // In radians. If my angle is this close to 0, I use the special-case paths + const double eps = 1e-8; + + // Here I special-case A and B near 0. I do this so avoid dividing by 0 in + // the /A and /B expressions. There are no /sin(A) and /sin(B) expressions, + // so I don't need to think about A ~ k pi + if(A.x < eps/2.) + { + // A ~ 0. I simplify + // + // cosC ~ + // + cosB + // - A*sinB * inner(r0,r1) / 4AB + // sinC r01 / 2C ~ + // + A*cosB* r0 / 2A + // + sinB * r1 / 2B + // + A*sinB * cross(r0,r1) / 4AB + // + // I have C = B + dB where dB ~ 0, so + // + // cosC ~ cos(B + dB) ~ cosB - dB sinB + // -> dB = A * inner(r0,r1) / 4AB = + // inner(r0,r1) / 4B + // -> C = B + inner(r0,r1) / 4B + // + // Now let's look at the axis expression. Assuming A ~ 0 + // + // sinC r01 / 2C ~ + // + A*cosB r0 / 2A + // + sinB r1 / 2B + // + A*sinB * cross(r0,r1) / 4AB + // -> + // sinC/C * r01 ~ + // + cosB r0 + // + sinB r1 / B + // + sinB * cross(r0,r1) / 2B + // + // I linearize the left-hand side: + // + // sinC/C = sin(B+dB)/(B+dB) ~ + // sinB/B + d( sinB/B )/dB dB = + // sinB/B + dB (B cosB - sinB) / B^2 + // + // So + // + // (sinB/B + dB (B cosB - sinB) / B^2) r01 ~ + // + cosB r0 + // + sinB r1 / B + // + sinB * cross(r0,r1) / 2B + // -> + // (sinB + dB (B cosB - sinB) / B) r01 ~ + // + cosB*B r0 + // + sinB r1 + // + sinB * cross(r0,r1) / 2 + // -> + // sinB (r01 - r1) + dB (B cosB - sinB) / B r01 ~ + // + cosB*B r0 + // + sinB * cross(r0,r1) / 2 + // + // I want to find the perturbation to give me r01 ~ r1 + deltar -> + // + // sinB deltar + dB (B cosB - sinB) / B (r1 + deltar) ~ + // + cosB*B r0 + // + sinB * cross(r0,r1) / 2 + // + // All terms here are linear or quadratic in r0. For tiny r0, I can + // ignore the quadratic terms: + // + // sinB deltar + dB (B cosB - sinB) / B r1 ~ + // + cosB*B r0 + // + sinB * cross(r0,r1) / 2 + // -> + // deltar ~ + // - dB (B/tanB - 1) / B r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // + // I substitute in the dB from above, and I simplify: + // + // deltar ~ + // - inner(r0,r1) / 4B (B/tanB - 1) / B r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // = + // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // + // So + // + // r01 = r1 + // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + if(B.x < eps) + { + // what if B is ALSO near 0? I simplify further + // + // lim(B->0) (B/tanB) = lim( 1 / sec^2 B) = 1. + // lim(B->0) d(B/tanB)/dB = 0 + // + // (B/tanB - 1) / 4B^2 = + // (B - tanB) / (4 B^2 tanB) + // lim(B->0) = 0 + // lim(B->0) d/dB = 0 + // + // So + // r01 = r1 + // + r0 + // + cross(r0,r1) / 2 + // + // Here I have linear and quadratic terms. With my tiny numbers, the + // quadratic terms can be ignored, so simply + // + // r01 = r0 + r1 + *r = *r0 + *r1; + return; + } + + const val_withgrad_t& B_over_tanB = B / B.tan(); + for(int i=0; i<3; i++) + (*r)[i] = + (*r1)[i] * (val_withgrad_t(1.0) + - inner * (B_over_tanB - 1.) / (B*B*4.)) + + (*r0)[i] * B_over_tanB + + cross[i] / 2.; + return; + } + else if(B.x < eps) + { + // B ~ 0. I simplify + // + // cosC = + // cosA - + // sinA*B * inner(r0,r1) / 4AB + // sinC r01 / 2C = + // sinA*r0 / 2A + + // cosA*B*r1 / 2B + + // sinA*B * cross(r0,r1) / 4AB + // + // I have C = A + dA where dA ~ 0, so + // + // cosC ~ cos(A + dA) ~ cosA - dA sinA + // -> dA = B * inner(r0,r1) / 4AB = + // = inner(r0,r1) / 4A + // -> C = A + inner(r0,r1) / 4A + // + // Now let's look at the axis expression. Assuming B ~ 0 + // + // sinC r01 / 2C = + // + sinA*r0 / 2A + // + cosA*B*r1 / 2B + // + sinA*B * cross(r0,r1) / 4AB + // -> + // sinC/C r01 = + // + sinA*r0 / A + // + cosA*r1 + // + sinA * cross(r0,r1) / 2A + // + // I linearize the left-hand side: + // + // sinC/C = sin(A+dA)/(A+dA) ~ + // sinA/A + d( sinA/A )/dA dA = + // sinA/A + dA (A cosA - sinA) / A^2 + // + // So + // + // (sinA/A + dA (A cosA - sinA) / A^2) r01 ~ + // + sinA*r0 / A + // + cosA*r1 + // + sinA * cross(r0,r1) / 2A + // -> + // (sinA + dA (A cosA - sinA) / A) r01 ~ + // + sinA*r0 + // + cosA*r1*A + // + sinA * cross(r0,r1) / 2 + // -> + // sinA (r01 - r0) + dA (A cosA - sinA) / A r01 ~ + // + cosA*A r1 + // + sinA * cross(r0,r1) / 2 + // + // + // I want to find the perturbation to give me r01 ~ r0 + deltar -> + // + // sinA deltar + dA (A cosA - sinA) / A (r0 + deltar) ~ + // + cosA*A r1 + // + sinA * cross(r0,r1) / 2 + // + // All terms here are linear or quadratic in r1. For tiny r1, I can + // ignore the quadratic terms: + // + // sinA deltar + dA (A cosA - sinA) / A r0 ~ + // + cosA*A r1 + // + sinA * cross(r0,r1) / 2 + // -> + // deltar ~ + // - dA (A/tanA - 1) / A r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + // + // I substitute in the dA from above, and I simplify: + // + // deltar ~ + // - inner(r0,r1) / 4A (A/tanA - 1) / A r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + // = + // - inner(r0,r1) (A/tanA - 1) / 4A^2 r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + // + // So + // + // r01 = r0 + // - inner(r0,r1) (A/tanA - 1) / 4A^2 r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + + // I don't have an if(A.x < eps){} case here; this is handled in + // the above if() branch + + const val_withgrad_t& A_over_tanA = A / A.tan(); + for(int i=0; i<3; i++) + (*r)[i] = + (*r0)[i] * (val_withgrad_t(1.0) + - inner * (A_over_tanA - 1.) / (A*A*4.)) + + (*r1)[i] * A_over_tanA + + cross[i] / 2.; + return; + } + + const vec_withgrad_t sincosA = A.sincos(); + const vec_withgrad_t sincosB = B.sincos(); + + const val_withgrad_t& sinA = sincosA.v[0]; + const val_withgrad_t& cosA = sincosA.v[1]; + const val_withgrad_t& sinB = sincosB.v[0]; + const val_withgrad_t& cosB = sincosB.v[1]; + + const val_withgrad_t& sinA_over_A = A.sinx_over_x(sinA); + const val_withgrad_t& sinB_over_B = B.sinx_over_x(sinB); + + val_withgrad_t cosC = + cosA*cosB - + sinA_over_A*sinB_over_B*inner/4.; + + for(int i=0; i<3; i++) + (*r)[i] = + sinA_over_A*cosB*(*r0)[i] + + sinB_over_B*cosA*(*r1)[i] + + sinA_over_A*sinB_over_B*cross[i]/2.; + + // To handle numerical fuzz + // cos(x ~ 0) ~ 1 - x^2/2 + if (cosC.x - 1.0 > -eps*eps/2.) + { + // special-case. C ~ 0 -> sin(C)/C ~ 1 -> r01 is already computed. We're + // done + } + // cos(x ~ pi) ~ cos(pi) + (x-pi)^2/2 (-cos(pi)) + // ~ -1 + (x-pi)^2/2 + else if(cosC.x + 1.0 < eps*eps/2. ) + { + // special-case. cos(C) ~ -1 -> C ~ pi. This corresponds to a full + // rotation: gamma = 2C ~ 2pi. I wrap it around to avoid dividing by + // 0. + // + // For any r = gamma n where mag(n)=1 I can use an equivalent r' = + // (gamma-2pi)n + // + // Here gamma = 2C so gamma-2pi = 2(C-pi) + + /* + I have cosC and u = r01 sin(C)/C (stored in the "r" variable) + + gamma = mag(r01) = 2C + mag(u) * C/sin(C) = 2C + mag(u) = 2 sin(C) + + r' = (mag(r01) - 2pi) * r01/mag(r01) + = (1 - 2pi/mag(r01)) * r01 + = (1 - 2pi/2C) * u C/sin(C) + = ((C - pi)/C) * u C/sin(C) + = (C - pi)/sin(C) * u + = -(pi - C)/sin(pi - C) * u + ~ -u + */ + for(int i=0; i<3; i++) + (*r)[i] *= -1.; + } + // Not-strictly-necessary special case. I would like to have C in + // [-pi/2,pi/2] instead of [0,pi]. This will produce a nicer-looking + // (smaller numbers) r01 + else if(cosC.x < 0.) + { + // Need to subtract a rotation + + // I have cos(C) and u (stored in "r") + // r01 = C/sin(C) u + // + // I want r01 - r01/mag(r01)*2pi + // = C/sin(C) u ( 1 - 2pi/mag(r01)) + // = C/sin(C) u ( 1 - pi/C ) + // = 1/sin(C) u ( C - pi ) + // = (C - pi)/sin(C) u + const val_withgrad_t C = cosC.acos(); + const val_withgrad_t sinC = (val_withgrad_t(1.) - cosC*cosC).sqrt(); + + for(int i=0; i<3; i++) + (*r)[i] *= (C - M_PI) / sinC; + } + else + { + // nominal case + const val_withgrad_t C = cosC.acos(); + const val_withgrad_t sinC = (val_withgrad_t(1.) - cosC*cosC).sqrt(); + + for(int i=0; i<3; i++) + (*r)[i] *= C / sinC; + } +} + +extern "C" +void mrcal_compose_r_full( // output + double* r_out, // (3,) array + int r_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + double* dr_dr1, // (3,3) array; may be NULL + int dr_dr1_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r_0, // (3,) array + int r_0_stride0, // in bytes. <= 0 means "contiguous" + const double* r_1, // (3,) array + int r_1_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(r_out, 3); + init_stride_2D(dr_dr0, 3,3); + init_stride_2D(dr_dr1, 3,3); + init_stride_1D(r_0, 3); + init_stride_1D(r_1, 3); + + if(dr_dr0 == NULL && dr_dr1 == NULL) + { + // no gradients + vec_withgrad_t<0, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, -1, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, -1, + r_1_stride0); + + vec_withgrad_t<0, 3> r01g; + compose_r_core<0>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + } + else if(dr_dr0 != NULL && dr_dr1 == NULL) + { + // r0 gradient only + vec_withgrad_t<3, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, 0, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, -1, + r_1_stride0); + + vec_withgrad_t<3, 3> r01g; + compose_r_core<3>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + r01g.extract_grad(dr_dr0, + 0,3, + 0, + dr_dr0_stride0, dr_dr0_stride1, + 3); + } + else if(dr_dr0 == NULL && dr_dr1 != NULL) + { + // r1 gradient only + vec_withgrad_t<3, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, -1, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, 0, + r_1_stride0); + + vec_withgrad_t<3, 3> r01g; + compose_r_core<3>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + r01g.extract_grad(dr_dr1, + 0,3, + 0, + dr_dr1_stride0, dr_dr1_stride1, + 3); + } + else + { + // r0 AND r1 gradients + vec_withgrad_t<6, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, 0, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, 3, + r_1_stride0); + + vec_withgrad_t<6, 3> r01g; + compose_r_core<6>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + r01g.extract_grad(dr_dr0, + 0,3, + 0, + dr_dr0_stride0, dr_dr0_stride1, + 3); + r01g.extract_grad(dr_dr1, + 3,3, + 0, + dr_dr1_stride0, dr_dr1_stride1, + 3); + } +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c b/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c new file mode 100644 index 0000000000..59f7738477 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c @@ -0,0 +1,1129 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +// Apparently I need this in MSVC to get constants +#define _USE_MATH_DEFINES + +#include +#include +#include + +#include "poseutils.h" +#include "strides.h" +#include "minimath/minimath.h" + +// All arrays stored in row-major order +// +// I have two different representations of pose transformations: +// +// - Rt is a concatenated (4,3) array: Rt = nps.glue(R,t, axis=-2). The +// transformation is R*x+t +// +// - rt is a concatenated (6) array: rt = nps.glue(r,t, axis=-1). The +// transformation is R*x+t where R = R_from_r(r) + + +// row vectors: vout = matmult(v,Mt) +// equivalent col vector expression: vout = matmult(M,v) +#define mul_vec3_gen33t_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1, \ + scale) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + scale * \ + (_P2(Mt,Mt_stride0,Mt_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,2)*_P1(v,v_stride0,2) ), \ + scale * \ + (_P2(Mt,Mt_stride0,Mt_stride1,1,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,2)*_P1(v,v_stride0,2) ), \ + scale * \ + (_P2(Mt,Mt_stride0,Mt_stride1,2,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,2)*_P1(v,v_stride0,2) ) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) +#define mul_vec3_gen33t_vout_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1) \ + mul_vec3_gen33t_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1, 1.0) +// row vectors: vout = scale*matmult(v,M) +#define mul_vec3_gen33_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + M, M_stride0, M_stride1, \ + scale) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + scale * \ + (_P2(M,M_stride0,M_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,0)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,0)*_P1(v,v_stride0,2)), \ + scale * \ + (_P2(M,M_stride0,M_stride1,0,1)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,1)*_P1(v,v_stride0,2)), \ + scale * \ + (_P2(M,M_stride0,M_stride1,0,2)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,2)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,2)*_P1(v,v_stride0,2)) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) +#define mul_vec3_gen33_vout_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1) \ + mul_vec3_gen33_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1, 1.0) + +// row vectors: vout = matmult(v,Mt) +// equivalent col vector expression: vout = matmult(M,v) +#define mul_vec3_gen33t_vaccum_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + _P1(vout,vout_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,2)*_P1(v,v_stride0,2), \ + _P1(vout,vout_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,2)*_P1(v,v_stride0,2), \ + _P1(vout,vout_stride0,2) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,2)*_P1(v,v_stride0,2) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) + +// row vectors: vout = scale*matmult(v,M) +#define mul_vec3_gen33_vaccum_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + M, M_stride0, M_stride1, \ + scale) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + _P1(vout,vout_stride0,0) + scale * \ + (_P2(M,M_stride0,M_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,0)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,0)*_P1(v,v_stride0,2)), \ + _P1(vout,vout_stride0,1) + scale * \ + (_P2(M,M_stride0,M_stride1,0,1)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,1)*_P1(v,v_stride0,2)), \ + _P1(vout,vout_stride0,2) + scale * \ + (_P2(M,M_stride0,M_stride1,0,2)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,2)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,2)*_P1(v,v_stride0,2)) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) + +// multiply two (3,3) matrices +static inline +void mul_gen33_gen33_vout_full(// output + double* m0m1, + int m0m1_stride0, int m0m1_stride1, + + // input + const double* m0, + int m0_stride0, int m0_stride1, + const double* m1, + int m1_stride0, int m1_stride1) +{ + /* needed for in-place operations */ + double outcopy2[9]; + + for(int i=0; i<3; i++) + // one row at a time + mul_vec3_gen33_vout_scaled_full(&outcopy2[i*3], sizeof(outcopy2[0]), + &_P2(m0 , m0_stride0, m0_stride1, i,0), m0_stride1, + m1, m1_stride0, m1_stride1, + 1.0); + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(m0m1, i,j) = outcopy2[3*i+j]; +} + +static inline +double inner3(const double* restrict a, + const double* restrict b) +{ + double s = 0.0; + for (int i=0; i<3; i++) s += a[i]*b[i]; + return s; +} + + + + +// Make an identity rotation or transformation +void mrcal_identity_R_full(double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(R, 3,3); + P2(R, 0,0) = 1.0; P2(R, 0,1) = 0.0; P2(R, 0,2) = 0.0; + P2(R, 1,0) = 0.0; P2(R, 1,1) = 1.0; P2(R, 1,2) = 0.0; + P2(R, 2,0) = 0.0; P2(R, 2,1) = 0.0; P2(R, 2,2) = 1.0; +} +void mrcal_identity_r_full(double* r, // (3,) array + int r_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(r, 3); + P1(r, 0) = 0.0; P1(r, 1) = 0.0; P1(r, 2) = 0.0; +} +void mrcal_identity_Rt_full(double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(Rt, 4,3); + mrcal_identity_R_full(Rt, Rt_stride0, Rt_stride1); + for(int i=0; i<3; i++) P2(Rt, 3, i) = 0.0; +} +void mrcal_identity_rt_full(double* rt, // (6,) array + int rt_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(rt, 6); + mrcal_identity_r_full(rt, rt_stride0); + for(int i=0; i<3; i++) P1(rt, i+3) = 0.0; +} + +void mrcal_rotate_point_R_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_R, // (3,3,3) array. May be NULL + int J_R_stride0, // in bytes. <= 0 means "contiguous" + int J_R_stride1, // in bytes. <= 0 means "contiguous" + int J_R_stride2, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* R, // (3,3) array. May be NULL + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_R corresponds + // to the input R + ) +{ + init_stride_1D(x_out, 3); + init_stride_3D(J_R, 3,3,3 ); + init_stride_2D(J_x, 3,3 ); + init_stride_2D(R, 3,3 ); + init_stride_1D(x_in, 3 ); + + if(inverted) + { + // transpose R + int tmp; + + tmp = R_stride0; + R_stride0 = R_stride1; + R_stride1 = tmp; + + tmp = J_R_stride1; + J_R_stride1 = J_R_stride2; + J_R_stride2 = tmp; + } + + if(J_R) + { + // out[i] = inner(R[i,:],in) + for(int i=0; i<3; i++) + { + int j=0; + for(; j a = R'b - R't +void mrcal_invert_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_in, // (4,3) array + int Rt_in_stride0, // in bytes. <= 0 means "contiguous" + int Rt_in_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(Rt_out, 4,3); + init_stride_2D(Rt_in, 4,3); + + // transpose(R). Extra stuff to make in-place operations work + for(int i=0; i<3; i++) + P2(Rt_out,i,i) = P2(Rt_in,i,i); + for(int i=0; i<3; i++) + for(int j=i+1; j<3; j++) + { + double tmp = P2(Rt_in,i,j); + P2(Rt_out,i,j) = P2(Rt_in,j,i); + P2(Rt_out,j,i) = tmp; + } + + // -transpose(R)*t + mul_vec3_gen33t_vout_scaled_full(&P2(Rt_out,3,0), Rt_out_stride1, + &P2(Rt_in, 3,0), Rt_in_stride1, + Rt_out, Rt_out_stride0, Rt_out_stride1, + -1.0); +} + +// Invert an rt transformation +// +// b = rotate(a) + t -> a = invrotate(b) - invrotate(t) +// +// drout_drin is not returned: it is always -I +// drout_dtin is not returned: it is always 0 +void mrcal_invert_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dtout_drin, // (3,3) array + int dtout_drin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_drin_stride1, // in bytes. <= 0 means "contiguous" + double* dtout_dtin, // (3,3) array + int dtout_dtin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_dtin_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_in, // (6,) array + int rt_in_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(rt_out, 6); + // init_stride_2D(dtout_drin, 3,3); + init_stride_2D(dtout_dtin, 3,3); + init_stride_1D(rt_in, 6); + + // r uses an angle-axis representation, so to undo a rotation r, I can apply + // a rotation -r (same axis, equal and opposite angle) + for(int i=0; i<3; i++) + P1(rt_out,i) = -P1(rt_in,i); + + mrcal_rotate_point_r_full( &P1(rt_out,3), rt_out_stride0, + dtout_drin, dtout_drin_stride0, dtout_drin_stride1, + dtout_dtin, dtout_dtin_stride0, dtout_dtin_stride1, + + // input + rt_out, rt_out_stride0, + &P1(rt_in,3), rt_in_stride0, + false); + for(int i=0; i<3; i++) + P1(rt_out,3+i) *= -1.; + + if(dtout_dtin) + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(dtout_dtin,i,j) *= -1.; +} + + +// Compose two Rt transformations +// R0*(R1*x + t1) + t0 = +// (R0*R1)*x + R0*t1+t0 +void mrcal_compose_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_0, // (4,3) array + int Rt_0_stride0, // in bytes. <= 0 means "contiguous" + int Rt_0_stride1, // in bytes. <= 0 means "contiguous" + const double* Rt_1, // (4,3) array + int Rt_1_stride0, // in bytes. <= 0 means "contiguous" + int Rt_1_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(Rt_out, 4,3); + init_stride_2D(Rt_0, 4,3); + init_stride_2D(Rt_1, 4,3); + + // for in-place operation + double t0[] = { P2(Rt_0,3,0), + P2(Rt_0,3,1), + P2(Rt_0,3,2) }; + + // t <- R0*t1 + mul_vec3_gen33t_vout_full(&P2(Rt_out,3,0), Rt_out_stride1, + &P2(Rt_1, 3,0), Rt_1_stride1, + Rt_0, Rt_0_stride0, Rt_0_stride1); + + // R <- R0*R1 + mul_gen33_gen33_vout_full( Rt_out, Rt_out_stride0, Rt_out_stride1, + Rt_0, Rt_0_stride0, Rt_0_stride1, + Rt_1, Rt_1_stride0, Rt_1_stride1 ); + + // t <- R0*t1+t0 + for(int i=0; i<3; i++) + P2(Rt_out,3,i) += t0[i]; +} + +// Compose two rt transformations. It is assumed that we're getting no gradients +// at all or we're getting ALL the gradients: only dr_r0 is checked for NULL +// +// dr_dt0 is not returned: it is always 0 +// dr_dt1 is not returned: it is always 0 +// dt_dr1 is not returned: it is always 0 +// dt_dt0 is not returned: it is always the identity matrix +void mrcal_compose_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_r0, // (3,3) array; may be NULL + int dr_r0_stride0, // in bytes. <= 0 means "contiguous" + int dr_r0_stride1, // in bytes. <= 0 means "contiguous" + double* dr_r1, // (3,3) array; may be NULL + int dr_r1_stride0, // in bytes. <= 0 means "contiguous" + int dr_r1_stride1, // in bytes. <= 0 means "contiguous" + double* dt_r0, // (3,3) array; may be NULL + int dt_r0_stride0, // in bytes. <= 0 means "contiguous" + int dt_r0_stride1, // in bytes. <= 0 means "contiguous" + double* dt_t1, // (3,3) array; may be NULL + int dt_t1_stride0, // in bytes. <= 0 means "contiguous" + int dt_t1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_0, // (6,) array + int rt_0_stride0, // in bytes. <= 0 means "contiguous" + const double* rt_1, // (6,) array + int rt_1_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(rt_out, 6); + init_stride_2D(dr_r0, 3,3); + init_stride_2D(dr_r1, 3,3); + init_stride_2D(dt_r0, 3,3); + init_stride_2D(dt_t1, 3,3); + init_stride_1D(rt_0, 6); + init_stride_1D(rt_1, 6); + + // r0 (r1 x + t1) + t0 = r0 r1 x + r0 t1 + t0 + // -> I want (r0 r1, r0 t1 + t0) + + + // to make in-place operation work + double rt0[6]; + for(int i=0; i<6; i++) + rt0[i] = P1(rt_0, i); + + // Compute r01 + mrcal_compose_r_full( rt_out, rt_out_stride0, + dr_r0, dr_r0_stride0, dr_r0_stride1, + dr_r1, dr_r1_stride0, dr_r1_stride1, + + rt_0, rt_0_stride0, + rt_1, rt_1_stride0); + + + // t01 <- r0 t1 + mrcal_rotate_point_r_full( &P1(rt_out,3), rt_out_stride0, + dt_r0, dt_r0_stride0, dt_r0_stride1, + dt_t1, dt_t1_stride0, dt_t1_stride1, + + rt0, -1, + &P1(rt_1,3), rt_1_stride0, + + false ); + // t01 <- r0 t1 + t0 + for(int i=0; i<3; i++) + P1(rt_out,3+i) += rt0[3+i]; +} + +void mrcal_compose_r_tinyr0_gradientr0_full( // output + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r_1, // (3,) array + int r_1_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(dr_dr0, 3, 3); + init_stride_1D(r_1, 3); + + // All the comments and logic appear in compose_r_core() in + // poseutils-uses-autodiff.cc. This is a special-case function with + // manually-computed gradients (because I want to make sure they're fast) + double norm2_r1 = 0.0; + for(int i=0; i<3; i++) + norm2_r1 += P1(r_1,i)*P1(r_1,i); + + if(norm2_r1 < 2e-8*2e-8) + { + // Both vectors are tiny, so I have r01 = r0 + r1, and the gradient is + // an identity matrix + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(dr_dr0,i,j) = i==j ? 1.0 : 0.0; + return; + } + + // I'm computing + // R(angle=gamma, axis=n) = + // compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) + // where + // A = alpha/2 + // B = beta /2 + + // I have + // r01 = r1 + // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // + // I differentiate: + // + // dr01/dr0 = + // - outer(r1,r1) (B/tanB - 1) / 4B^2 + // + B/tanB I + // - skew_symmetric(r1) / 2 + double B = sqrt(norm2_r1) / 2.; + double B_over_tanB = B / tan(B); + + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(dr_dr0,i,j) = + - P1(r_1,i)*P1(r_1,j) * (B_over_tanB - 1.) / (4.*B*B); + for(int i=0; i<3; i++) + P2(dr_dr0,i,i) += + B_over_tanB; + + P2(dr_dr0,0,1) -= -P1(r_1,2)/2.; + P2(dr_dr0,0,2) -= P1(r_1,1)/2.; + P2(dr_dr0,1,0) -= P1(r_1,2)/2.; + P2(dr_dr0,1,2) -= -P1(r_1,0)/2.; + P2(dr_dr0,2,0) -= -P1(r_1,1)/2.; + P2(dr_dr0,2,1) -= P1(r_1,0)/2.; +} + +void mrcal_compose_r_tinyr1_gradientr1_full( // output + double* dr_dr1, // (3,3) array; may be NULL + int dr_dr1_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r_0, // (3,) array + int r_0_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(dr_dr1, 3, 3); + init_stride_1D(r_0, 3); + + // All the comments and logic appear in compose_r_core() in + // poseutils-uses-autodiff.cc. This is a special-case function with + // manually-computed gradients (because I want to make sure they're fast) + double norm2_r0 = 0.0; + for(int i=0; i<3; i++) + norm2_r0 += P1(r_0,i)*P1(r_0,i); + + if(norm2_r0 < 2e-8*2e-8) + { + // Both vectors are tiny, so I have r01 = r0 + r1, and the gradient is + // an identity matrix + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(dr_dr1,i,j) = i==j ? 1.0 : 0.0; + return; + } + + // I'm computing + // R(angle=gamma, axis=n) = + // compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) + // where + // A = alpha/2 + // B = beta /2 + + // I have + // r01 = r0 + // - inner(r0,r1) (A/tanA - 1) / 4A^2 r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + // + // I differentiate: + // + // dr01/dr1 = + // - outer(r0,r0) (A/tanA - 1) / 4A^2 + // + A/tanA I + // + skew_symmetric(r0) / 2 + double A = sqrt(norm2_r0) / 2.; + double A_over_tanA = A / tan(A); + + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(dr_dr1,i,j) = + - P1(r_0,i)*P1(r_0,j) * (A_over_tanA - 1.) / (4.*A*A); + for(int i=0; i<3; i++) + P2(dr_dr1,i,i) += + A_over_tanA; + + P2(dr_dr1,0,1) += -P1(r_0,2)/2.; + P2(dr_dr1,0,2) += P1(r_0,1)/2.; + P2(dr_dr1,1,0) += P1(r_0,2)/2.; + P2(dr_dr1,1,2) += -P1(r_0,0)/2.; + P2(dr_dr1,2,0) += -P1(r_0,1)/2.; + P2(dr_dr1,2,1) += P1(r_0,0)/2.; +} + + +void mrcal_r_from_R_full( // output + double* r, // (3,) vector + int r_stride0, // in bytes. <= 0 means "contiguous" + double* J, // (3,3,3) array. Gradient. May be NULL + int J_stride0, // in bytes. <= 0 means "contiguous" + int J_stride1, // in bytes. <= 0 means "contiguous" + int J_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(r, 3); + init_stride_3D(J, 3,3,3); + init_stride_2D(R, 3,3); + + + // Looking at https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula the + // Rodrigues rotation formula for th rad rotation around unit axis v is + // + // R = I + sin(th) V + (1 - cos(th)) V^2 + // + // where V = skew_symmetric(v): + // + // [ 0 -v2 v1] + // V(v) = [ v2 0 -v0] + // [-v1 v0 0] + // + // and + // + // v(V) = [-V12, V02, -V01] + // + // I, V^2 are symmetric; V is anti-symmetric. So R - Rt = 2 sin(th) V + // + // Let's define + // + // [ R21 - R12 ] + // u = [ -R20 + R02 ] = v(R) - v(Rt) + // [ R10 - R01 ] + // + // From the above equations we see that u = 2 sin(th) v. So I compute the + // axis v = u/mag(u). I want th in [0,pi] so I can't compute th from u since + // there's an ambiguity: sin(th) = sin(pi-th). So instead, I compute th from + // trace(R) = 1 + 2*cos(th) + // + // There's an extra wrinkle here. Computing the axis from mag(u) only works + // if sin(th) != 0. So there are two special cases that must be handled: th + // ~ 0 and th ~ 180. If th ~ 0, then the axis doesn't matter and r ~ 0. If + // th ~ 180 then the axis DOES matter, and we need special logic. + const double tr = P2(R,0,0) + P2(R,1,1) + P2(R,2,2); + const double u[3] = + { + P2(R,2,1) - P2(R,1,2), + P2(R,0,2) - P2(R,2,0), + P2(R,1,0) - P2(R,0,1) + }; + + const double costh = (tr - 1.) / 2.; + + // In radians. If my angle is this close to 0, I use the special-case paths + const double eps = 1e-8; + + // near 0 we have norm2u ~ 4 th^2 + const double norm2u = + u[0]*u[0] + + u[1]*u[1] + + u[2]*u[2]; + if(// both conditions to handle roundoff error + norm2u > 4. * eps*eps && + 1. - fabs(costh) > eps*eps/2. ) + { + // normal path + const double th = acos(costh); + for(int i=0; i<3; i++) + P1(r,i) = u[i]/sqrt(norm2u) * th; + } + else if(costh > 0) + { + // small th. Can't divide by it. But I can look at the limit. + // + // u / (2 sinth)*th = u/2 *th/sinth ~ u/2 + for(int i=0; i<3; i++) + P1(r,i) = u[i] / 2.; + } + else + { + // cos(th) < 0. So th ~ +-180 = +-180 + dth where dth ~ 0. And I have + // + // R = I + sin(th) V + (1 - cos(th) ) V^2 + // = I + sin(+-180 + dth) V + (1 - cos(+-180 + dth)) V^2 + // = I - sin(dth) V + (1 + cos(dth)) V^2 + // ~ I - dth V + 2 V^2 + // + // Once again, I, V^2 are symmetric; V is anti-symmetric. So + // + // R - Rt = 2 sin(th) V + // = -2 sin(dth) V + // = -2 dth V + // I want + // + // r = th v + // = dth v +- 180deg v + // + // r = v((R - Rt) / -2.) +- 180deg v + // = u/-2 +- 180deg v + // + // Now we need v; let's look at the symmetric parts: + // + // R + Rt = 2 I + 4 V^2 + //-> V^2 = (R + Rt)/4 - I/2 + // + // [ 0 -v2 v1] + // V(v) = [ v2 0 -v0] + // [-v1 v0 0] + // + // [ -(v1^2+v2^2) v0 v1 v0 v2 ] + // V^2(v) = [ v0 v1 -(v0^2+v2^2) v1 v2 ] + // [ v0 v2 v1 v2 -(v0^2+v1^2) ] + // + // I want v be a unit vector. Can I assume that? From above: + // + // tr(V^2) = -2 norm2(v) + // + // So I want to assume that tr(V^2) = -2. The earlier expression had + // + // R + Rt = 2 I + 4 V^2 + // + // -> tr(R + Rt) = tr(2 I + 4 V^2) + // -> tr(V^2) = (tr(R + Rt) - 6)/4 + // = (2 tr(R) - 6)/4 + // = (1 + 2*cos(th) - 3)/2 + // = -1 + cos(th) + // + // Near th ~ 180deg, this is -2 as required. So we can assume that + // mag(v)=1: + // + // [ v0^2 - 1 v0 v1 v0 v2 ] + // V^2(v) = [ v0 v1 v1^2 - 1 v1 v2 ] + // [ v0 v2 v1 v2 v2^2 - 1 ] + // + // So + // + // v^2 = 1 + diag(V^2) + // = 1 + 2 diag(R)/4 - I/2 + // = 1 + diag(R)/2 - 1/2 + // = (1 + diag(R))/2 + for(int i=0; i<3; i++) + P1(r,i) = u[i] / -2.; + + // Now r += pi v + const double vsq[3] = + { + (P2(R,0,0) + 1.) /2., + (P2(R,1,1) + 1.) /2., + (P2(R,2,2) + 1.) /2. + }; + // This is abs(v) initially + double v[3] = {}; + for(int i=0; i<3; i++) + if(vsq[i] > 0.0) + v[i] = sqrt(vsq[i]); + else + { + // round-off sets this at 0; it's already there. Leave it + } + + // Now I need to get the sign of each individual value. Overall, the + // sign of the vector v doesn't matter. I set the sign of a notably + // non-zero abs(v[i]) to >0, and go from there. + + // threshold can be anything notably > 0. I'd like to encourage the same + // branch to always be taken, so I set the thresholds relatively low + if( v[0] > 0.1) + { + // I leave v[0]>0. + // V^2[0,1] must have the same sign as v1 + // V^2[0,2] must have the same sign as v2 + if( (P2(R,0,1) + P2(R,1,0)) < 0 ) v[1] *= -1.; + if( (P2(R,0,2) + P2(R,2,0)) < 0 ) v[2] *= -1.; + } + else if(v[1] > 0.1) + { + // I leave v[1]>0. + // V^2[1,0] must have the same sign as v0 + // V^2[1,2] must have the same sign as v2 + if( (P2(R,1,0) + P2(R,0,1)) < 0 ) v[0] *= -1.; + if( (P2(R,1,2) + P2(R,2,1)) < 0 ) v[2] *= -1.; + } + else + { + // I leave v[2]>0. + // V^2[2,0] must have the same sign as v0 + // V^2[2,1] must have the same sign as v1 + if( (P2(R,2,0) + P2(R,0,2)) < 0 ) v[0] *= -1.; + if( (P2(R,2,1) + P2(R,1,2)) < 0 ) v[1] *= -1.; + } + + for(int i=0; i<3; i++) + P1(r,i) += v[i] * M_PI; + } + + + if(J != NULL) + { + // Not all (3,3) matrices R are valid rotations, and I make sure to evaluate + // the gradient in the subspace defined by the opposite operation: R_from_r + // + // I'm assuming a flattened R.shape = (9,) everywhere here + // + // - I compute r = r_from_R(R) + // + // - R',dR/dr = R_from_r(r, get_gradients = True) + // R' should match R. This method assumes that. + // + // - We have + // dR = dR/dr dr + // dr = dr/dR dR + // so + // dr/dR dR/dr = I + // + // - dR/dr has shape (9,3). In response to perturbations in r, R moves in a + // rank-3 subspace: this is the local subspace of valid rotation + // matrices. The dr/dR we seek should be limited to that subspace as + // well. So dr/dR = M (dR/dr)' for some 3x3 matrix M + // + // - Combining those two I get + // dr/dR = M (dR/dr)' + // dr/dR dR/dr = M (dR/dr)' dR/dr + // I = M (dR/dr)' dR/dr + // -> + // M = inv( (dR/dr)' dR/dr ) + // -> + // dr/dR = M (dR/dr)' + // = inv( (dR/dr)' dR/dr ) (dR/dr)' + // = pinv(dR/dr) + + // share memory + union + { + // Unused. The tests make sure this is the same as R + double R_roundtrip[3*3]; + double det_inv_dRflat_drT__dRflat_dr[6]; + } m; + + double dRflat_dr[9*3]; // inverse gradient + + mrcal_R_from_r_full( // outputs + m.R_roundtrip,0,0, + dRflat_dr, 0,0,0, + + // input + r,r_stride0 ); + + ////// transpose(dRflat_dr) * dRflat_dr + // 3x3 symmetric matrix; packed,dense storage; row-first + double dRflat_drT__dRflat_dr[6] = {}; + int i_result = 0; + for(int i=0; i<3; i++) + for(int j=i;j<3;j++) + { + for(int k=0; k<9; k++) + dRflat_drT__dRflat_dr[i_result] += + dRflat_dr[k*3 + i]* + dRflat_dr[k*3 + j]; + i_result++; + } + + ////// inv( transpose(dRflat_dr) * dRflat_dr ) + // 3x3 symmetric matrix; packed,dense storage; row-first + double inv_det = 1./cofactors_sym3(dRflat_drT__dRflat_dr, m.det_inv_dRflat_drT__dRflat_dr); + + ////// inv( transpose(dRflat_dr) * dRflat_dr ) transpose(dRflat_dr) + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + { + // computing dr/dR[i,j] + double dr[3] = {}; + mul_vec3_sym33_vout_scaled( &dRflat_dr[3*(j + 3*i)], m.det_inv_dRflat_drT__dRflat_dr, + dr, + inv_det); + for(int k=0; k<3; k++) + P3(J, k,i,j) = dr[k]; + } + } +} + +// Compute a non-unique rotation to map a given vector to [0,0,1] +// See docstring for mrcal.R_aligned_to_vector() for details +void mrcal_R_aligned_to_vector(// out + double* R, + // in + const double* v) +{ + double magv = sqrt(v[0]*v[0] + + v[1]*v[1] + + v[2]*v[2]); + + double* x = &R[0*3]; + double* y = &R[1*3]; + double* z = &R[2*3]; + + for(int i=0; i<3; i++) + { + x[i] = 0.0; + z[i] = v[i]/magv; + } + + double inner_x_z = 0.; + if(fabs(z[0]) < .9) + { + x[0] = 1.; + inner_x_z = z[0]; + } + else + { + x[1] = 1.; + inner_x_z = z[1]; + } + + for(int i=0; i<3; i++) + x[i] -= inner_x_z*z[i]; + + double magx = sqrt(x[0]*x[0] + + x[1]*x[1] + + x[2]*x[2]); + + for(int i=0; i<3; i++) + x[i] /= magx; + + // y = cross(z,x); + y[0] = z[1]*x[2] - z[2]*x[1]; + y[1] = z[2]*x[0] - z[0]*x[2]; + y[2] = z[0]*x[1] - z[1]*x[0]; +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp new file mode 100644 index 0000000000..ca8ea012f4 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp @@ -0,0 +1,1032 @@ +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 + +#include "autodiff.hh" + +extern "C" { +#include "triangulation.h" +} + +template +static +bool +triangulate_assume_intersect( // output + vec_withgrad_t& m, + + // inputs. camera-0 coordinates + const vec_withgrad_t& v0, + const vec_withgrad_t& v1, + const vec_withgrad_t& t01) +{ + // I take two 3D rays that are assumed to intersect, and return the + // intersection point. Results are undefined if these rays actually + // don't intersect + + // Each pixel observation represents a ray in 3D: + // + // k0 v0 = t01 + k1 v1 + // + // t01 = [v0 -v1] k + // + // This is over-determined: k has 2DOF, but I have 3 equations. I know that + // the vectors intersect, so I can use 2 axes only, which makes the problem + // uniquely determined. Let's pick the 2 axes to use. The "forward" + // direction (z) is dominant, so let's use that. For the second axis, let's + // use whichever is best numerically: biggest abs(det), so that I divide by + // something as far away from 0 as possible. I have + // + double fabs_det_xz = fabs(-v0.v[0].x*v1.v[2].x + v0.v[2].x*v1.v[0].x); + double fabs_det_yz = fabs(-v0.v[1].x*v1.v[2].x + v0.v[2].x*v1.v[1].x); + + // If using xz, I have: + // + // k = 1/(-v0[0]*v1[2] + v0[2]*v1[0]) * [-v1[2] v1[0] ] t01 + // [-v0[2] v0[0] ] + // [0] -> [1] if using yz + val_withgrad_t k0; + if(fabs_det_xz > fabs_det_yz) + { + // xz + if(fabs_det_xz <= 1e-10) + return false; + + val_withgrad_t det = v1.v[0]*v0.v[2] - v0.v[0]*v1.v[2]; + k0 = (t01.v[2]*v1.v[0] - t01.v[0]*v1.v[2]) / det; + if(k0.x <= 0.0) + return false; + bool k1_negative = (t01.v[2].x*v0.v[0].x > t01.v[0].x*v0.v[2].x) ^ (det.x > 0); + if(k1_negative) + return false; + +#if 0 + val_withgrad_t k1 = (t01.v[2]*v0.v[0] - t01.v[0]*v0.v[2]) / det; + vec_withgrad_t m2 = v1*k1 + t01; + m2 -= m; + double d2 = m2.v[0].x*m2.v[0].x + m2.v[1].x*m2.v[1].x + m2.v[2].x*m2.v[2].x; + fprintf(stderr, "diff: %f\n", d2); +#endif + } + else + { + // yz + if(fabs_det_yz <= 1e-10) + return false; + + val_withgrad_t det = v1.v[1]*v0.v[2] - v0.v[1]*v1.v[2]; + k0 = (t01.v[2]*v1.v[1] - t01.v[1]*v1.v[2]) / det; + if(k0.x <= 0.0) + return false; + bool k1_negative = (t01.v[2].x*v0.v[1].x > t01.v[1].x*v0.v[2].x) ^ (det.x > 0); + if(k1_negative) + return false; + +#if 0 + val_withgrad_t k1 = (t01.v[2]*v0.v[1] - t01.v[1]*v0.v[2]) / det; + vec_withgrad_t m2 = v1*k1 + t01; + m2 -= m; + double d2 = m2.v[1].x*m2.v[1].x + m2.v[1].x*m2.v[1].x + m2.v[2].x*m2.v[2].x; + fprintf(stderr, "diff: %f\n", d2); +#endif + } + + m = v0 * k0; + + return true; +} + + +// #warning "These all have NGRAD=9, which is inefficient: some/all of the requested gradients could be NULL" + +// Basic closest-approach-in-3D routine +extern "C" +mrcal_point3_t +mrcal_triangulate_geometric(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + // This is the basic 3D-geometry routine. I find the point in 3D that + // minimizes the distance to each of the observation rays. This is simple, + // but not as accurate as we'd like. All the other methods have lower + // biases. See the Lee-Civera papers for details: + // + // Paper that compares all methods implemented here: + // "Triangulation: Why Optimize?", Seong Hun Lee and Javier Civera. + // https://arxiv.org/abs/1907.11917 + // + // Earlier paper that doesn't have mid2 or wmid2: + // "Closed-Form Optimal Two-View Triangulation Based on Angular Errors", + // Seong Hun Lee and Javier Civera. ICCV 2019. + // + // Each pixel observation represents a ray in 3D. The best + // estimate of the 3d position of the point being observed + // is the point nearest to both these rays + // + // Let's say I have a ray from the origin to v0, and another ray from t01 + // to v1 (v0 and v1 aren't necessarily normal). Let the nearest points on + // each ray be k0 and k1 along each ray respectively: E = norm2(t01 + k1*v1 + // - k0*v0): + // + // dE/dk0 = 0 = inner(t01 + k1*v1 - k0*v0, -v0) + // dE/dk1 = 0 = inner(t01 + k1*v1 - k0*v0, v1) + // + // -> t01.v0 + k1 v0.v1 = k0 v0.v0 + // -t01.v1 + k0 v0.v1 = k1 v1.v1 + // + // -> [ v0.v0 -v0.v1] [k0] = [ t01.v0] + // [ -v0.v1 v1.v1] [k1] = [-t01.v1] + // + // -> [k0] = 1/(v0.v0 v1.v1 -(v0.v1)**2) [ v1.v1 v0.v1][ t01.v0] + // [k1] [ v0.v1 v0.v0][-t01.v1] + // + // I return the midpoint: + // + // x = (k0 v0 + t01 + k1 v1)/2 + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + val_withgrad_t<9> dot_v0v0 = v0.norm2(); + val_withgrad_t<9> dot_v1v1 = v1.norm2(); + val_withgrad_t<9> dot_v0v1 = v0.dot(v1); + val_withgrad_t<9> dot_v0t = v0.dot(t01); + val_withgrad_t<9> dot_v1t = v1.dot(t01); + + val_withgrad_t<9> denom = dot_v0v0*dot_v1v1 - dot_v0v1*dot_v0v1; + if(-1e-10 <= denom.x && denom.x <= 1e-10) + return (mrcal_point3_t){0}; + + val_withgrad_t<9> denom_recip = val_withgrad_t<9>(1.)/denom; + val_withgrad_t<9> k0 = denom_recip * (dot_v1v1*dot_v0t - dot_v0v1*dot_v1t); + if(k0.x <= 0.0) return (mrcal_point3_t){0}; + val_withgrad_t<9> k1 = denom_recip * (dot_v0v1*dot_v0t - dot_v0v0*dot_v1t); + if(k1.x <= 0.0) return (mrcal_point3_t){0}; + + vec_withgrad_t<9,3> m = (v0*k0 + v1*k1 + t01) * 0.5; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3, 0, + 3*sizeof(double), sizeof(double), + 3); + +#if 0 + MSG("intersecting..."); + MSG("v0 = (%.20f,%.20f,%.20f)", v0[0].x,v0[1].x,v0[2].x); + MSG("t01 = (%.20f,%.20f,%.20f)", t01[0].x,t01[1].x,t01[2].x); + MSG("v1 = (%.20f,%.20f,%.20f)", v1[0].x,v1[1].x,v1[2].x); + MSG("intersection = (%.20f,%.20f,%.20f) dist %f", + m.v[0].x,m.v[1].x,m.v[2].x, + sqrt( m.dot(m).x)); +#endif + + return _m; +} + +// Minimize L2 pinhole reprojection error. Described in "Triangulation Made +// Easy", Peter Lindstrom, IEEE Conference on Computer Vision and Pattern +// Recognition, 2010. +extern "C" +mrcal_point3_t +mrcal_triangulate_lindstrom(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_dRt01, + + // inputs + + // not-necessarily normalized vectors in the LOCAL + // coordinate system. This is different from the other + // triangulation routines + const mrcal_point3_t* _v0_local, + const mrcal_point3_t* _v1_local, + const mrcal_point3_t* _Rt01) +{ + // This is an implementation of the algorithm described in "Triangulation + // Made Easy", Peter Lindstrom, IEEE Conference on Computer Vision and + // Pattern Recognition, 2010. A copy of this paper is available in this repo + // in docs/TriangulationLindstrom.pdf. The implementation here is the niter2 + // routine in Listing 3. There's a higher-level implemented-in-python test + // in analyses/triangulation.py + // + // A simpler, but less-accurate way of doing is lives in + // triangulate_direct() + + // I'm looking at wikipedia for the Essential matrix definition: + // + // https://en.wikipedia.org/wiki/Essential_matrix + // + // and at Lindstrom's paper. Note that THEY HAVE DIFFERENT DEFINITIONS OF E + // + // I stick to Lindstrom's convention here. He has (section 2, equation 3) + // + // E = cross(t) R + // transpose(x0) E x1 = 0 + // + // What are R and t? + // + // x0' cross(t) R x1 = 0 + // x0' cross(t) R (R10 x0 + t10) = 0 + // + // So Lindstrom has R = R01 -> + // + // x0' cross(t) R01 (R10 x0 + t10) = 0 + // x0' cross(t) (x0 + R01 t10) = 0 + // x0' cross(t) R01 t10 = 0 + // + // This holds if Lindstrom has R01 t10 = +- t + // + // Note that if x1 = R10 x0 + t10 then x0 = R01 x1 - R01 t10 + // + // So I let t = t01 + // + // Thus he's multiplying cross(t01) and R01: + // + // E = cross(t01) R01 + // = cross(t01) R10' + + // cross(t01) = np.array(((0, -t01[2], t01[1]), + // ( t01[2], 0, -t01[0]), + // (-t01[1], t01[0], 0))); + + vec_withgrad_t<18,3> v0 (_v0_local->xyz, 0); + vec_withgrad_t<18,3> v1 (_v1_local->xyz, 3); + vec_withgrad_t<18,9> R01(_Rt01 ->xyz, 6); + vec_withgrad_t<18,3> t01(_Rt01[3] .xyz, 15); + + val_withgrad_t<18> E[9] = { R01[6]*t01[1] - R01[3]*t01[2], + R01[7]*t01[1] - R01[4]*t01[2], + R01[8]*t01[1] - R01[5]*t01[2], + + R01[0]*t01[2] - R01[6]*t01[0], + R01[1]*t01[2] - R01[7]*t01[0], + R01[2]*t01[2] - R01[8]*t01[0], + + R01[3]*t01[0] - R01[0]*t01[1], + R01[4]*t01[0] - R01[1]*t01[1], + R01[5]*t01[0] - R01[2]*t01[1] }; + + // Paper says to rescale x0,x1 such that their last element is 1.0. + // I don't even store it + val_withgrad_t<18> x0[2] = { v0[0]/v0[2], v0[1]/v0[2] }; + val_withgrad_t<18> x1[2] = { v1[0]/v1[2], v1[1]/v1[2] }; + + // for debugging +#if 0 + { + fprintf(stderr, "E:\n"); + for(int i=0; i<3; i++) + fprintf(stderr, "%f %f %f\n", E[0 + 3*i].x, E[1 + 3*i].x, E[2 + 3*i].x); + double Ex1[3] = { E[0].x*x1[0].x + E[1].x*x1[1].x + E[2].x, + E[3].x*x1[0].x + E[4].x*x1[1].x + E[5].x, + E[6].x*x1[0].x + E[7].x*x1[1].x + E[8].x }; + double x0Ex1 = Ex1[0]*x0[0].x + Ex1[1]*x0[1].x + Ex1[2]; + fprintf(stderr, "conj before: %f\n", x0Ex1); + } +#endif + + // Now I implement the algorithm. x0 here is x in the paper; x1 here + // is x' in the paper + + // Step 1. n = nps.matmult(x1, nps.transpose(E))[:2] + val_withgrad_t<18> n[2]; + n[0] = E[0]*x1[0] + E[1]*x1[1] + E[2]; + n[1] = E[3]*x1[0] + E[4]*x1[1] + E[5]; + + + // Step 2. nn = nps.matmult(x0, E)[:2] + val_withgrad_t<18> nn[2]; + nn[0] = E[0]*x0[0] + E[3]*x0[1] + E[6]; + nn[1] = E[1]*x0[0] + E[4]*x0[1] + E[7]; + + // Step 3. a = nps.matmult( n, E[:2,:2], nps.transpose(nn) ).ravel() + val_withgrad_t<18> a = + n[0]*E[0]*nn[0] + + n[0]*E[1]*nn[1] + + n[1]*E[3]*nn[0] + + n[1]*E[4]*nn[1]; + + // Step 4. b = 0.5*( nps.inner(n,n) + nps.inner(nn,nn) ) + val_withgrad_t<18> b = (n [0]*n [0] + n [1]*n [1] + + nn[0]*nn[0] + nn[1]*nn[1]) * 0.5; + + // Step 5. c = nps.matmult(x0, E, nps.transpose(x1)).ravel() + val_withgrad_t<18> n_2 = + E[6]*x1[0] + + E[7]*x1[1] + + E[8]; + val_withgrad_t<18> c = + n[0]*x0[0] + + n[1]*x0[1] + + n_2; + + // Step 6. d = np.sqrt( b*b - a*c ) + val_withgrad_t<18> d = (b*b - a*c).sqrt(); + + // Step 7. l = c / (b+d) + val_withgrad_t<18> l = c / (b + d); + + // Step 8. dx = l*n + val_withgrad_t<18> dx[2] = { l * n[0], l * n[1] }; + + // Step 9. dxx = l*nn + val_withgrad_t<18> dxx[2] = { l * nn[0], l * nn[1] }; + + // Step 10. n -= nps.matmult(dxx, nps.transpose(E[:2,:2])) + n[0] = n[0] - E[0]*dxx[0] - E[1]*dxx[1] ; + n[1] = n[1] - E[3]*dxx[0] - E[4]*dxx[1] ; + + // Step 11. nn -= nps.matmult(dx, E[:2,:2]) + nn[0] = nn[0] - E[0]*dx[0] - E[3]*dx[1] ; + nn[1] = nn[1] - E[1]*dx[0] - E[4]*dx[1] ; + + // Step 12. l *= 2*d/( nps.inner(n,n) + nps.inner(nn,nn) ) + val_withgrad_t<18> bb = (n [0]*n [0] + n [1]*n [1] + + nn[0]*nn[0] + nn[1]*nn[1]) * 0.5; + l = l/d * bb; + + // Step 13. dx = l*n + dx[0] = l * n[0]; + dx[1] = l * n[1]; + + // Step 14. dxx = l*nn + dxx[0] = l * nn[0]; + dxx[1] = l * nn[1]; + + // Step 15 + v0.v[0] = x0[0] - dx[0]; + v0.v[1] = x0[1] - dx[1]; + v0.v[2] = val_withgrad_t<18>(1.0); + + // Step 16 + v1.v[0] = x1[0] - dxx[0]; + v1.v[1] = x1[1] - dxx[1]; + v1.v[2] = val_withgrad_t<18>(1.0); + + // for debugging +#if 0 + { + double Ex1[3] = { E[0].x*v1[0].x + E[1].x*v1[1].x + E[2].x, + E[3].x*v1[0].x + E[4].x*v1[1].x + E[5].x, + E[6].x*v1[0].x + E[7].x*v1[1].x + E[8].x }; + double x0Ex1 = Ex1[0]*v0[0].x + Ex1[1]*v0[1].x + Ex1[2]; + fprintf(stderr, "conj after: %f\n", x0Ex1); + } +#endif + + // Construct v0, v1 in a common coord system + vec_withgrad_t<18,3> Rv1; + Rv1.v[0] = R01.v[0]*v1.v[0] + R01.v[1]*v1.v[1] + R01.v[2]*v1.v[2]; + Rv1.v[1] = R01.v[3]*v1.v[0] + R01.v[4]*v1.v[1] + R01.v[5]*v1.v[2]; + Rv1.v[2] = R01.v[6]*v1.v[0] + R01.v[7]*v1.v[1] + R01.v[8]*v1.v[2]; + + // My two 3D rays now intersect exactly, and I use compute the intersection + // with that assumption + vec_withgrad_t<18,3> m; + if(!triangulate_assume_intersect(m, v0, Rv1, t01)) + return (mrcal_point3_t){0}; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dRt01 != NULL) + m.extract_grad (_dm_dRt01->xyz, 6,12,0, + 12*sizeof(double), sizeof(double), + 3); + return _m; +} + +// Minimize L1 angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. +extern "C" +mrcal_point3_t +mrcal_triangulate_leecivera_l1(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + val_withgrad_t<9> dot_v0v0 = v0.norm2(); + val_withgrad_t<9> dot_v1v1 = v1.norm2(); + val_withgrad_t<9> dot_v0t = v0.dot(t01); + val_withgrad_t<9> dot_v1t = v1.dot(t01); + + // I pick a bath based on which len(cross(normalize(m),t)) is larger: which + // of m0 and m1 is most perpendicular to t. I can use a simpler dot product + // here instead: the m that is most perpendicular to t will have the + // smallest dot product. + // + // len(cross(m0/len(m0), t)) < len(cross(m1/len(m1), t)) ~ + // len(cross(v0/len(v0), t)) < len(cross(v1/len(v1), t)) ~ + // abs(dot(v0/len(v0), t)) > abs(dot(v1/len(v1), t)) ~ + // (dot(v0/len(v0), t))^2 > (dot(v1/len(v1), t))^2 ~ + // (dot(v0, t))^2 norm2(v1) > (dot(v1, t))^2 norm2(v0) ~ + if(dot_v0t.x*dot_v0t.x * dot_v1v1.x > dot_v1t.x*dot_v1t.x * dot_v0v0.x ) + { + // Equation (12) + vec_withgrad_t<9,3> n1 = cross<9>(v1, t01); + v0 -= n1 * v0.dot(n1)/n1.norm2(); + } + else + { + // Equation (13) + vec_withgrad_t<9,3> n0 = cross<9>(v0, t01); + v1 -= n0 * v1.dot(n0)/n0.norm2(); + } + + // My two 3D rays now intersect exactly, and I use compute the intersection + // with that assumption + + vec_withgrad_t<9,3> m; + if(!triangulate_assume_intersect(m, v0, v1, t01)) + return (mrcal_point3_t){0}; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + return _m; +} + +// Minimize L-infinity angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. +extern "C" +mrcal_point3_t +mrcal_triangulate_leecivera_linf(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + v0 /= v0.mag(); + v1 /= v1.mag(); + + vec_withgrad_t<9,3> na = cross<9>(v0 + v1, t01); + vec_withgrad_t<9,3> nb = cross<9>(v0 - v1, t01); + + vec_withgrad_t<9,3>& n = + ( na.norm2().x > nb.norm2().x ) ? + na : nb; + + v0 -= n * v0.dot(n)/n.norm2(); + v1 -= n * v1.dot(n)/n.norm2(); + + // My two 3D rays now intersect exactly, and I use compute the intersection + // with that assumption + vec_withgrad_t<9,3> m; + if(!triangulate_assume_intersect(m, v0, v1, t01)) + return (mrcal_point3_t){0}; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + return _m; +} + +// This is called "cheirality" in Lee and Civera's papers +template +static bool chirality(// output + val_withgrad_t& improvement0, + val_withgrad_t& improvement1, + val_withgrad_t& improvement01, + + // input + const val_withgrad_t& l0, + const vec_withgrad_t& v0, + const val_withgrad_t& l1, + const vec_withgrad_t& v1, + const vec_withgrad_t& t01) +{ + double len2_nominal = 0.0; + double len2; + + improvement0 = val_withgrad_t(); + improvement1 = val_withgrad_t(); + improvement01 = val_withgrad_t(); + + for(int i=0; i<3; i++) + { + val_withgrad_t x_nominal = ( l1*v1.v[i] + t01.v[i]) - l0*v0.v[i]; + val_withgrad_t x0 = ( l1*v1.v[i] + t01.v[i]) + l0*v0.v[i]; + val_withgrad_t x1 = (-l1*v1.v[i] + t01.v[i]) - l0*v0.v[i]; + val_withgrad_t x01 = (-l1*v1.v[i] + t01.v[i]) + l0*v0.v[i]; + + improvement0 += x0 *x0 - x_nominal*x_nominal; + improvement1 += x1 *x1 - x_nominal*x_nominal; + improvement01 += x01*x01 - x_nominal*x_nominal; + } + + return + improvement0.x > 0.0 && + improvement1.x > 0.0 && + improvement01.x > 0.0; +} + +// version without reporting the improvement values +template +static bool chirality(const val_withgrad_t& l0, + const vec_withgrad_t& v0, + const val_withgrad_t& l1, + const vec_withgrad_t& v1, + const vec_withgrad_t& t01) +{ + val_withgrad_t improvement0; + val_withgrad_t improvement1; + val_withgrad_t improvement01; + return chirality(improvement0, improvement1, improvement01, + l0,v0,l1,v1,t01); +} + +// The "Mid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +extern "C" +mrcal_point3_t +mrcal_triangulate_leecivera_mid2(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + val_withgrad_t<9> p_norm2_recip = val_withgrad_t<9>(1.0) / cross_norm2<9>(v0, v1); + + val_withgrad_t<9> l0 = (cross_norm2<9>(v1, t01) * p_norm2_recip).sqrt(); + val_withgrad_t<9> l1 = (cross_norm2<9>(v0, t01) * p_norm2_recip).sqrt(); + + if(!chirality(l0, v0, l1, v1, t01)) + return (mrcal_point3_t){0}; + + vec_withgrad_t<9,3> m = (v0*l0 + t01+v1*l1) / 2.0; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + return _m; +} + +extern "C" +bool +_mrcal_triangulate_leecivera_mid2_is_convergent(// inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + mrcal_point3_t p = mrcal_triangulate_leecivera_mid2(NULL,NULL,NULL, + _v0,_v1,_t01); + return !(p.x == 0.0 && + p.y == 0.0 && + p.z == 0.0); +} + +// The "wMid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and +// Javier Civera. https://arxiv.org/abs/1907.11917 +extern "C" +mrcal_point3_t +mrcal_triangulate_leecivera_wmid2(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + // Unlike Mid2 I need to normalize these here to make the math work. l0 and + // l1 now have units of m, and I weigh by 1/l0 and 1/l1 + v0 /= v0.mag(); + v1 /= v1.mag(); + + val_withgrad_t<9> p_mag_recip = val_withgrad_t<9>(1.0) / cross_mag<9>(v0, v1); + + val_withgrad_t<9> l0 = cross_mag<9>(v1, t01) * p_mag_recip; + val_withgrad_t<9> l1 = cross_mag<9>(v0, t01) * p_mag_recip; + + if(!chirality(l0, v0, l1, v1, t01)) + return (mrcal_point3_t){0}; + + vec_withgrad_t<9,3> m = (v0*l0*l1 + t01*l0 + v1*l0*l1) / (l0 + l1); + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + return _m; +} + +static +val_withgrad_t<6> +angle_error__assume_small(const vec_withgrad_t<6,3>& v0, + const vec_withgrad_t<6,3>& v1) +{ + const val_withgrad_t<6> inner00 = v0.norm2(); + const val_withgrad_t<6> inner11 = v1.norm2(); + const val_withgrad_t<6> inner01 = v0.dot(v1); + + val_withgrad_t<6> costh = inner01 / (inner00*inner11).sqrt(); + if(costh.x < 0.0) + // Could happen with barely-divergent rays + costh *= val_withgrad_t<6>(-1.0); + + // The angle is assumed small, so cos(th) ~ 1 - th*th/2. + // -> th ~ sqrt( 2*(1 - cos(th)) ) + val_withgrad_t<6> th_sq = costh*(-2.) + 2.; + + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: temporary hack to avoid dividing by 0" +#endif + if(th_sq.x < 1e-21) + { + return val_withgrad_t<6>(); + } + + + if(th_sq.x < 0) + // To handle roundoff errors + th_sq.x = 0; + + return th_sq.sqrt(); +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: look at numerical issues that will results in sqrt(<0)" +// #warning "triangulated-solve: look at behavior near 0 where dsqrt/dx -> inf" +#endif +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: maybe exposing the triangulated-error C function is OK? I'm already exposing the Python function" +#endif + + +static +double relu(double x, double knee) +{ + /* Smooth function ~ x>0 ? x : 0 + + Three modes + - x < 0: 0 + - 0 < x < knee: k*x^2 + - knee < x: x + eps + + At the transitions I want the function and the first derivative + to be continuous. At the knee I want d/dx = 1. So 2*k*knee = 1 -> + k = 1/(2*knee). k * knee^2 = knee + eps -> eps = knee * (k*knee - + 1) = -knee/2 + */ + if(x <= 0) return 0.0; + if(knee <= x) return x - knee/2.0; + + double k = 1. / (2*knee); + return k * x*x; +} + +static +val_withgrad_t<6> sigmoid(val_withgrad_t<6> x, double knee) +{ + /* Smooth function maps to 0..1 + + Modes + - x < 0: 0 + - 0 < x < knee: smooth interpolation + - knee < x: 1 + */ + if(x.x <= 0 ) return 0.0; + if(knee <= x.x) return 1.0; + + // transition at (x - knee/2.) < 0 + // f(x) = a x^2 + b x + c + // f(-knee/2.) = 0 + // f(0) = 1/2 + // f'(-knee/2.) = 0 + if(x.x < knee/2.0) + { + double b = 2./knee; + double a = b/knee; + double c = 1./2.; + return c + (x.x - knee/2.)*(b + (x.x - knee/2.)*a); + } + + // transition at (x - knee/2.) > 0 + // f(x) = a x^2 + b x + c + // f(knee/2.) = 1 + // f'(knee/2.) = 0 + // f(0) = 1/2 + { + double b = 2./knee; + double a = -b/knee; + double c = 1./2.; + return c + (x.x - knee/2.)*(b + (x.x - knee/2.)*a); + } +} + +// Internal function used in the optimization. This uses +// mrcal_triangulate_leecivera_mid2(), but contains logic in the divergent-ray +// case more appropriate for the optimization loop +extern "C" +double +_mrcal_triangulated_error(// outputs + mrcal_point3_t* _derr_dv1, + mrcal_point3_t* _derr_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + ////////////////////////// Copy of mrcal_triangulate_leecivera_mid2(). I + ////////////////////////// extend it + + // Implementation here is a bit different: I don't propagate the gradient in + // respect to v0 + + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<6,3> v0 (_v0 ->xyz, -1); // No gradient. Hopefully the + // compiler will collapse this + // aggressively + vec_withgrad_t<6,3> v1 (_v1 ->xyz, 0); + vec_withgrad_t<6,3> t01(_t01->xyz, 3); + + val_withgrad_t<6> p_norm2_recip = val_withgrad_t<6>(1.0) / cross_norm2<6>(v0, v1); + + val_withgrad_t<6> l0 = (cross_norm2<6>(v1, t01) * p_norm2_recip).sqrt(); + val_withgrad_t<6> l1 = (cross_norm2<6>(v0, t01) * p_norm2_recip).sqrt(); + + vec_withgrad_t<6,3> m = (v0*l0 + t01+v1*l1) / 2.0; + + // I compute the angle between the triangulated point and one of the + // observation rays, and I double this to measure from ray to ray + + // This is a fit error, which should be small. A small-angle cos() + // approximation is valid, unless the models don't fit at all. In which + // case a cos() error is the least of our issues + val_withgrad_t<6> err = angle_error__assume_small( v0, m ) * 2.; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: what happens when the rays are exactly parallel? Make sure the numerics remain happy. They don't: I divide by cross(v0,v1) ~ 0" +#endif + +#if 0 + // original method + if(!chirality(l0, v0, l1, v1, t01)) + { + // The rays diverge. This is aphysical, but an incorrect (i.e. + // not-yet-converged) geometry can cause this. Even if the optimization + // has converged, this can still happen if pixel noise or an incorrect + // feature association bumped converging rays to diverge. + // + // An obvious thing do wo would be to return the distance to the + // vanishing point. This creates a yaw bias however: barely convergent + // rays have zero effect on yaw, but barely divergent rays have a strong + // effect on yaw + // + // Goals: + // + // - There should be no qualitative change in the cost function as rays + // cross over from convergent to divergent. Low-error, parallel-ish + // rays look out to infinity, which means that these define yaw very + // poorly, and would affect the pitch, roll only. Yaw is what controls + // divergence, so if random noise makes rays diverge, we should use + // the error as before, to set our pitch, roll + // + // - Very divergent rays are bogus, and I do apply a penalty factor + // based on the divergence. This penalty factor begins to kick in only + // past a certain point, so there's no effect at the transition. This + // transition point should be connected to the outlier rejection + // threshold. + val_withgrad_t<6> err_to_vanishing_point = angle_error__assume_small(v0, v1); + + // I have three modes: + // + // - err_to_vanishing_point < THRESHOLD_DIVERGENT_LOWER + // I attribute the error to random noise, and simply report the + // reprojection error as before, ignoring the divergence. This will + // barely affect the yaw, but will pull the pitch and roll in the + // desired directions + // + // - err_to_vanishing_point > THRESHOLD_DIVERGENT_UPPER + // I add a term to pull the observation towards the vanishing point. + // This affects the yaw primarily, and does not touch the pitch and + // roll very much, since I don't have reliable information about them + // here + // + // - err_to_vanishing_point between THRESHOLD_DIVERGENT_LOWER and _UPPER + // I linearly the scale on this divergence term from 0 to 1 + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: set reasonable thresholds" +#endif +#define THRESHOLD_DIVERGENT_LOWER 3.0e-4 +#define THRESHOLD_DIVERGENT_UPPER 6.0e-4 + + if(err_to_vanishing_point.x <= THRESHOLD_DIVERGENT_LOWER) + { + // We're barely divergent. Just do the normal thing + } + else if(err_to_vanishing_point.x >= THRESHOLD_DIVERGENT_UPPER) + { + // we're VERY divergent. Add another cost term: + // the distance to the vanishing point +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: temporary testing logic" +#endif +#if defined DIVERGENT_COST_ONLY && DIVERGENT_COST_ONLY + err = err_to_vanishing_point; +#else + err += err_to_vanishing_point; +#endif + err.x += 200.0; + } + else + { + // linearly interpolate. As + // err_to_vanishing_point lower->upper we + // produce k 0->1 + val_withgrad_t<6> k = + (err_to_vanishing_point - THRESHOLD_DIVERGENT_LOWER) / + (THRESHOLD_DIVERGENT_UPPER - THRESHOLD_DIVERGENT_LOWER); + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: temporary testing logic" +#endif +#if defined DIVERGENT_COST_ONLY && DIVERGENT_COST_ONLY + err = k*err_to_vanishing_point + (val_withgrad_t<6>(1.0)-k)*err; +#else + err += k*err_to_vanishing_point; +#endif + err.x += 100.0; + } + } + +#else + + // new method + val_withgrad_t<6> improvement0; + val_withgrad_t<6> improvement1; + val_withgrad_t<6> improvement01; + if(!chirality(improvement0, improvement1, improvement01, + l0, v0, l1, v1, t01)) + { + val_withgrad_t<6> err_to_vanishing_point = angle_error__assume_small(v0, v1); + + err += + err_to_vanishing_point * (sigmoid(-improvement0, 3.0) + + sigmoid(-improvement1, 3.0) + + sigmoid(-improvement01, 3.0)); + } + +#endif + + if(_derr_dv1 != NULL) + memcpy(_derr_dv1->xyz, + &err.j[0], + 3*sizeof(double)); + if(_derr_dt01 != NULL) + memcpy(_derr_dt01->xyz, + &err.j[3], + 3*sizeof(double)); + + return err.x; +} diff --git a/wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h b/wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h new file mode 100644 index 0000000000..0a36d4702d --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) Photon Vision. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#pragma once + +extern "C" { +// Seems to be missing C++ guards +#include + +} // extern "C" + +// Seems like these people don't properly extern-c their headers either +extern "C" { +#include +#include +} // extern "C" + +#include +#include +#include +#include +#include + +struct mrcal_result { + bool success; + std::vector intrinsics; + double rms_error; + std::vector residuals; + mrcal_calobject_warp_t calobject_warp; + int Noutliers_board; + // TODO standard devs + + mrcal_result() = default; + mrcal_result(bool success_, std::vector intrinsics_, + double rms_error_, std::vector residuals_, + mrcal_calobject_warp_t calobject_warp_, int Noutliers_board_) + : success{success_}, intrinsics{std::move(intrinsics_)}, + rms_error{rms_error_}, residuals{std::move(residuals_)}, + calobject_warp{calobject_warp_}, Noutliers_board{Noutliers_board_} {} + mrcal_result(mrcal_result &&) = delete; + ~mrcal_result(); +}; + +mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, + cv::Size boardSize, cv::Size imagerSize, + double squareSize, double focal_len_guess); + +std::unique_ptr mrcal_main( + // List, depth is ordered array observation[N frames, object_height, + // object_width] = [x,y, weight] weight<0 means ignored) + std::span observations_board, + // RT transform from camera to object + std::span frames_rt_toref, + // Chessboard size, in corners (not squares) + cv::Size calobjectSize, double boardSpacing, + // res, pixels + cv::Size cameraRes, + // focal length, in pixels + double focal_len_guess); + +enum class CameraLensModel { + LENSMODEL_OPENCV5 = 0, + LENSMODEL_OPENCV8, + LENSMODEL_STEREOGRAPHIC, + LENSMODEL_SPLINED_STEREOGRAPHIC +}; + +bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat, + const cv::Mat *distCoeffs, CameraLensModel lensModel, + // Extra stuff for splined stereographic models + uint16_t order, uint16_t Nx, uint16_t Ny, + uint16_t fov_x_deg); diff --git a/wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp b/wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp new file mode 100644 index 0000000000..31c0da37a1 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp @@ -0,0 +1,614 @@ +/* + * Copyright (C) Photon Vision. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#include "mrcal_wrapper.h" + + +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; + +class CholmodCtx { +public: + cholmod_common Common, *cc; + CholmodCtx() { + cc = &Common; + cholmod_l_start(cc); + } + + ~CholmodCtx() { cholmod_l_finish(cc); } +}; +static CholmodCtx cctx; + +#define BARF(...) std::fprintf(stderr, __VA_ARGS__) + +// forward declarations for functions borrowed from mrcal-pywrap +static mrcal_problem_selections_t construct_problem_selections( + int do_optimize_intrinsics_core, int do_optimize_intrinsics_distortions, + int do_optimize_extrinsics, int do_optimize_frames, + int do_optimize_calobject_warp, int do_apply_regularization, + int do_apply_outlier_rejection, int Ncameras_intrinsics, + int Ncameras_extrinsics, int Nframes, int Nobservations_board); + +bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel, + std::vector intrinsics, + bool do_check_layout); + +mrcal_point3_t* observations_point = nullptr; +mrcal_pose_t* + extrinsics_rt_fromref = nullptr; // Always zero for single camera, it seems? +mrcal_point3_t* points = nullptr; // Seems to always to be None for single camera? + +static std::unique_ptr mrcal_calibrate( + // List, depth is ordered array observation[N frames, object_height, + // object_width] = [x,y, weight] weight<0 means ignored) + std::span observations_board, + // RT transform from camera to object + std::span frames_rt_toref, + // Chessboard size, in corners (not squares) + Size calobjectSize, double calibration_object_spacing, + // res, pixels + Size cameraRes, + // solver options + mrcal_problem_selections_t problem_selections, + // seed intrinsics/cameramodel to optimize for + mrcal_lensmodel_t mrcal_lensmodel, std::vector intrinsics) { + // Number of board observations we've got. List of boards. in python, it's + // (number of chessboard pictures) x (rows) x (cos) x (3) + // hard-coded to 8, since that's what I've got below + int Nobservations_board = frames_rt_toref.size(); + + // Looks like this is hard-coded to 0 in Python for initial single-camera + // solve? + int Nobservations_point = 0; + + int calibration_object_width_n = + calobjectSize.width; // Object width, in # of corners + int calibration_object_height_n = + calobjectSize.height; // Object height, in # of corners + + // TODO set sizes and populate + int imagersize[] = {cameraRes.width, cameraRes.height}; + + mrcal_calobject_warp_t calobject_warp = {0, 0}; + + // int Nobservations_point_triangulated = 0; // no clue what this is + + int Npoints = 0; // seems like this is also unused? whack + int Npoints_fixed = 0; // seems like this is also unused? whack + + // Number of cameras to solve for intrinsics + int Ncameras_intrinsics = 1; + + // Hard-coded to match out 8 frames from above (borrowed from python) + std::vector indices_frame_camintrinsics_camextrinsics; + // Frame index, camera number, (camera number)-1??? + for (int i = 0; i < Nobservations_board; i++) { + indices_frame_camintrinsics_camextrinsics.push_back( + {static_cast(i), 0, -1}); + } + + // Pool is the raw observation backing array + mrcal_point3_t *c_observations_board_pool = (observations_board.data()); + mrcal_point3_t *c_observations_point_pool = observations_point; + + // Copy from board/point pool above, using some code borrowed from + // mrcal-pywrap + std::vector observations_board_data(Nobservations_board); + auto c_observations_board = observations_board_data.data(); + // Try to make sure we don't accidentally make a zero-length array or + // something stupid + std::vector + observations_point_data(std::max(Nobservations_point, 1)); + mrcal_observation_point_t* + c_observations_point = observations_point_data.data(); + + for (int i_observation = 0; i_observation < Nobservations_board; + i_observation++) { + int32_t iframe = + indices_frame_camintrinsics_camextrinsics.at(i_observation).x; + int32_t icam_intrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).y; + int32_t icam_extrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).z; + + c_observations_board[i_observation].icam.intrinsics = icam_intrinsics; + c_observations_board[i_observation].icam.extrinsics = icam_extrinsics; + c_observations_board[i_observation].iframe = iframe; + } + for (int i_observation = 0; i_observation < Nobservations_point; + i_observation++) { + int32_t i_point = + indices_frame_camintrinsics_camextrinsics.at(i_observation).x; + int32_t icam_intrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).y; + int32_t icam_extrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).z; + + c_observations_point[i_observation].icam.intrinsics = icam_intrinsics; + c_observations_point[i_observation].icam.extrinsics = icam_extrinsics; + c_observations_point[i_observation].i_point = i_point; + } + + int Ncameras_extrinsics = 0; // Seems to always be zero for single camera + int Nframes = + frames_rt_toref.size(); // Number of pictures of the object we've got + mrcal_observation_point_triangulated_t *observations_point_triangulated = NULL; + // NULL; + + if (!lensmodel_one_validate_args(&mrcal_lensmodel, intrinsics, false)) { + auto ret = std::make_unique(); + ret->success = false; + return ret; + } + + int Nstate = mrcal_num_states( + Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, + Nobservations_board, problem_selections, &mrcal_lensmodel); + + int Nmeasurements = mrcal_num_measurements( + Nobservations_board, Nobservations_point, + observations_point_triangulated, + 0, // hard-coded to 0 + calibration_object_width_n, calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, + problem_selections, &mrcal_lensmodel); + + // OK, now we should have everything ready! Just some final setup and then + // call optimize + + // Residuals + std::vector b_packed_final(Nstate); + auto c_b_packed_final = b_packed_final.data(); + + std::vector x_final(Nmeasurements); + auto c_x_final = x_final.data(); + + // Seeds + double *c_intrinsics = intrinsics.data(); + mrcal_pose_t *c_extrinsics = extrinsics_rt_fromref; + mrcal_pose_t *c_frames = frames_rt_toref.data(); + mrcal_point3_t *c_points = points; + mrcal_calobject_warp_t *c_calobject_warp = &calobject_warp; + + // in + int *c_imagersizes = imagersize; + auto point_min_range = -1.0, point_max_range = -1.0; + mrcal_problem_constants_t problem_constants = { + .point_min_range = point_min_range, .point_max_range = point_max_range}; + int verbose = 0; + + auto stats = mrcal_optimize( + NULL, -1, c_x_final, Nmeasurements * sizeof(double), c_intrinsics, + c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, + Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, + c_observations_board, c_observations_point, Nobservations_board, + Nobservations_point, + observations_point_triangulated, -1, + c_observations_board_pool, c_observations_point_pool, &mrcal_lensmodel, c_imagersizes, + problem_selections, &problem_constants, calibration_object_spacing, + calibration_object_width_n, calibration_object_height_n, verbose, false); + + std::vector residuals = {c_x_final, c_x_final + Nmeasurements}; + return std::make_unique( + true, intrinsics, stats.rms_reproj_error__pixels, residuals, + calobject_warp, stats.Noutliers_board); +} + +struct MrcalSolveOptions { + // If true, we solve for the intrinsics core. Applies only to those models + // that HAVE a core (fx,fy,cx,cy) + int do_optimize_intrinsics_core; + + // If true, solve for the non-core lens parameters + int do_optimize_intrinsics_distortions; + + // If true, solve for the geometry of the cameras + int do_optimize_extrinsics; + + // If true, solve for the poses of the calibration object + int do_optimize_frames; + + // If true, optimize the shape of the calibration object + int do_optimize_calobject_warp; + + // If true, apply the regularization terms in the solver + int do_apply_regularization; + + // Whether to try to find NEW outliers. The outliers given on + // input are respected regardless + int do_apply_outlier_rejection; +}; + +// lifted from mrcal-pywrap.c. Restrict a given selection to only valid options +// License for mrcal-pywrap.c: +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +static mrcal_problem_selections_t +construct_problem_selections(MrcalSolveOptions s, int Ncameras_intrinsics, + int Ncameras_extrinsics, int Nframes, + int Nobservations_board) { + // By default we optimize everything we can + if (s.do_optimize_intrinsics_core < 0) + s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0; + if (s.do_optimize_intrinsics_distortions < 0) + s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0; + if (s.do_optimize_extrinsics < 0) + s.do_optimize_extrinsics = Ncameras_extrinsics > 0; + if (s.do_optimize_frames < 0) + s.do_optimize_frames = Nframes > 0; + if (s.do_optimize_calobject_warp < 0) + s.do_optimize_calobject_warp = Nobservations_board > 0; + return { + .do_optimize_intrinsics_core = + static_cast(s.do_optimize_intrinsics_core), + .do_optimize_intrinsics_distortions = + static_cast(s.do_optimize_intrinsics_distortions), + .do_optimize_extrinsics = static_cast(s.do_optimize_extrinsics), + .do_optimize_frames = static_cast(s.do_optimize_frames), + .do_optimize_calobject_warp = + static_cast(s.do_optimize_calobject_warp), + .do_apply_regularization = static_cast(s.do_apply_regularization), + .do_apply_outlier_rejection = + static_cast(s.do_apply_outlier_rejection), + // .do_apply_regularization_unity_cam01 = false + }; +} + +bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel, + std::vector intrinsics, + bool do_check_layout) { + int NlensParams = mrcal_lensmodel_num_params(mrcal_lensmodel); + int NlensParams_have = intrinsics.size(); + if (NlensParams != NlensParams_have) { + BARF("intrinsics.shape[-1] MUST be %d. Instead got %d", NlensParams, + NlensParams_have); + return false; + } + + return true; +} + +mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, + Size boardSize, Size imagerSize, double squareSize, + double focal_len_guess) { + using std::vector, std::runtime_error; + + if (!c_observations_board_pool) { + throw runtime_error("board was null"); + } + + double fx = focal_len_guess; + double fy = fx; + double cx = (imagerSize.width / 2.0) - 0.5; + double cy = (imagerSize.height / 2.0) - 0.5; + + vector objectPoints; + vector imagePoints; + + // Fill in object/image points + for (int i = 0; i < boardSize.height; i++) { + for (int j = 0; j < boardSize.width; j++) { + auto &corner = c_observations_board_pool[i * boardSize.width + j]; + // weight<0 means ignored -- filter these out + if (corner.z >= 0) { + imagePoints.emplace_back(corner.x, corner.y); + objectPoints.push_back(Point3f(j * squareSize, i * squareSize, 0)); + } + } + } + + { + // convert from stereographic to pinhole to match python + std::vector mrcal_imagepts(imagePoints.size()); + std::transform( + imagePoints.begin(), imagePoints.end(), mrcal_imagepts.begin(), + [](const auto &pt) { return mrcal_point2_t{.x = pt.x, .y = pt.y}; }); + + mrcal_lensmodel_t model{.type = MRCAL_LENSMODEL_STEREOGRAPHIC}; + std::vector out(imagePoints.size()); + const double intrinsics[] = {fx, fy, cx, cy}; + bool ret = mrcal_unproject(out.data(), mrcal_imagepts.data(), + mrcal_imagepts.size(), &model, intrinsics); + if (!ret) { + std::cerr << "couldn't unproject!" << std::endl; + } + model = {.type = MRCAL_LENSMODEL_PINHOLE}; + mrcal_project(mrcal_imagepts.data(), NULL, NULL, out.data(), out.size(), + &model, intrinsics); + + std::transform(mrcal_imagepts.begin(), mrcal_imagepts.end(), + imagePoints.begin(), + [](const auto &pt) { return Point2d{pt.x, pt.y}; }); + } + + // Initial guess at intrinsics + Mat cameraMatrix = (Mat_(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); + Mat distCoeffs = Mat(4, 1, CV_64FC1, Scalar(0)); + + Mat_ rvec, tvec; + vector objectPoints3; + for (auto a : objectPoints) + objectPoints3.push_back(Point3f(a.x, a.y, 0)); + + // for (auto& o : objectPoints) std::cout << o << std::endl; + // for (auto& i : imagePoints) std::cout << i << std::endl; + // std::cout << "cam mat\n" << cameraMatrix << std::endl; + // std::cout << "distortion: " << distCoeffs << std::endl; + + solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, + false, SOLVEPNP_ITERATIVE); + + return mrcal_pose_t{.r = {.x = rvec(0), .y = rvec(1), .z = rvec(2)}, + .t = {.x = tvec(0), .y = tvec(1), .z = tvec(2)}}; +} + +mrcal_result::~mrcal_result() { + // cholmod_l_free_sparse(&Jt, cctx.cc); + return; +} + +// Code taken from mrcal, license: +// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. +// Government sponsorship acknowledged. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +std::unique_ptr mrcal_main( + // List, depth is ordered array observation[N frames, object_height, + // object_width] = [x,y, weight] weight<0 means ignored) + std::span observations_board, + // RT transform from camera to object + std::span frames_rt_toref, + // Chessboard size, in corners (not squares) + Size calobjectSize, double calibration_object_spacing, + // res, pixels + Size cameraRes, double focal_length_guess) { + + std::unique_ptr result; + + { + // stereographic initial guess for intrinsics + double cx = (cameraRes.width / 2.0) - 0.5; + double cy = (cameraRes.height / 2.0) - 0.5; + std::vector intrinsics = {focal_length_guess, focal_length_guess, + cx, cy}; + + std::cout << "Initial solve (geometry only)" << std::endl; + + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = false, + .do_optimize_intrinsics_distortions = false, + .do_optimize_extrinsics = false, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = false, + .do_apply_outlier_rejection = false}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + + // And run calibration. This should mutate frames_rt_toref in place + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, intrinsics); + } + + { + std::cout + << "Initial solve (geometry and LENSMODEL_STEREOGRAPHIC core only)" + << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = false, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = false, + .do_apply_outlier_rejection = false}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, result->intrinsics); + } + + { + // Now we've got a seed, expand intrinsics to our target model + // see + // https://github.com/dkogan/mrcal/blob/33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2/mrcal-calibrate-cameras#L533 + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + // num distortion params + int nparams = mrcal_lensmodel_num_params(&mrcal_lensmodel); + std::vector intrinsics(nparams); + + // copy core in + std::copy(result->intrinsics.begin(), result->intrinsics.end(), + intrinsics.begin()); + + // and generate random distortion + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-0.5, 0.5); + + int nDistortion = nparams - 4; + std::vector seedDistortions(nDistortion); + + for (int j = 0; j < seedDistortions.size(); j++) { + if (j < 5) + seedDistortions[j] = dis(gen) * 2.0 * 1e-6; + else + seedDistortions[j] = dis(gen) * 2.0 * 1e-9; + } + + // copy distortion into our big intrinsics array + std::copy(seedDistortions.begin(), seedDistortions.end(), + intrinsics.begin() + result->intrinsics.size()); + + std::cout + << "Optimizing everything except board warp from seeded intrinsics" + << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = true, + .do_apply_outlier_rejection = true}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, intrinsics); + } + + { + std::cout << "Final, full solve" << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = true, + .do_apply_regularization = true, + .do_apply_outlier_rejection = true}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, result->intrinsics); + } + + return result; +} + +bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat, + const cv::Mat *distCoeffs, CameraLensModel lensModel, + // Extra stuff for splined stereographic models + uint16_t order, uint16_t Nx, uint16_t Ny, + uint16_t fov_x_deg) { + mrcal_lensmodel_t mrcal_lensmodel; + switch (lensModel) { + case CameraLensModel::LENSMODEL_OPENCV5: + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV5; + break; + case CameraLensModel::LENSMODEL_OPENCV8: + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + break; + case CameraLensModel::LENSMODEL_STEREOGRAPHIC: + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + break; + case CameraLensModel::LENSMODEL_SPLINED_STEREOGRAPHIC: + mrcal_lensmodel.type = MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC; + + /* Maximum degree of each 1D polynomial. This is almost certainly 2 */ + /* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */ + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order = order; + /* The horizontal field of view. Not including fov_y. It's proportional with + * Ny and Nx */ + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.fov_x_deg = + fov_x_deg; + /* We have a Nx by Ny grid of control points */ + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx = Nx; + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny = Ny; + break; + default: + std::cerr << "Unknown lensmodel\n"; + return false; + } + + if (!(dst->cols == 2 && dst->cols == 2)) { + std::cerr << "Bad input array size\n"; + return false; + } + if (!(dst->type() == CV_64FC2 && dst->type() == CV_64FC2)) { + std::cerr << "Bad input type -- need CV_64F\n"; + return false; + } + if (!(dst->isContinuous() && dst->isContinuous())) { + std::cerr << "Bad input array -- need continuous\n"; + return false; + } + + // extract intrinsics core from opencv camera matrix + double fx = cameraMat->at(0, 0); + double fy = cameraMat->at(1, 1); + double cx = cameraMat->at(0, 2); + double cy = cameraMat->at(1, 2); + + // Core, distortion coefficients concatenated + int NlensParams = mrcal_lensmodel_num_params(&mrcal_lensmodel); + std::vector intrinsics(NlensParams); + intrinsics[0] = fx; + intrinsics[1] = fy; + intrinsics[2] = cx; + intrinsics[3] = cy; + for (size_t i = 0; i < distCoeffs->cols; i++) { + intrinsics[i + 4] = distCoeffs->at(i); + } + + // input points in the distorted image pixel coordinates + mrcal_point2_t *in = reinterpret_cast(dst->data); + // vec3 observation vectors defined up-to-length + std::vector out(dst->rows); + + mrcal_unproject(out.data(), in, dst->rows, &mrcal_lensmodel, + intrinsics.data()); + + // The output is defined "up-to-length" + // Let's project through pinhole again + + // Output points in pinhole pixel coordinates + mrcal_point2_t *pinhole_pts = reinterpret_cast(dst->data); + + size_t bound = dst->rows; + for (size_t i = 0; i < bound; i++) { + // from mrcal-project-internal/pinhole model + mrcal_point3_t &p = out[i]; + + double z_recip = 1. / p.z; + double x = p.x * z_recip; + double y = p.y * z_recip; + + pinhole_pts[i].x = x * fx + cx; + pinhole_pts[i].y = y * fy + cy; + } + + return true; +} diff --git a/wpical/src/main/native/win/wpical.ico b/wpical/src/main/native/win/wpical.ico new file mode 100644 index 0000000000..0d82d10298 Binary files /dev/null and b/wpical/src/main/native/win/wpical.ico differ diff --git a/wpical/src/main/native/win/wpical.rc b/wpical/src/main/native/win/wpical.rc new file mode 100644 index 0000000000..e8b7112c3f --- /dev/null +++ b/wpical/src/main/native/win/wpical.rc @@ -0,0 +1 @@ +IDI_ICON1 ICON "wpical.ico" diff --git a/wpical/src/test/native/cpp/main.cpp b/wpical/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..a2b90c5913 --- /dev/null +++ b/wpical/src/test/native/cpp/main.cpp @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/wpical/src/test/native/cpp/test_calibrate.cpp b/wpical/src/test/native/cpp/test_calibrate.cpp new file mode 100644 index 0000000000..c185face6f --- /dev/null +++ b/wpical/src/test/native/cpp/test_calibrate.cpp @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +#include + +#include +#include + +const std::string projectRootPath = PROJECT_ROOT_PATH; +const std::string calSavePath = + projectRootPath.substr(0, + projectRootPath.find("/src/main/native/resources")) + + "/build"; +cameracalibration::CameraModel cameraModel = { + .intrinsic_matrix = Eigen::Matrix::Identity(), + .distortion_coefficients = Eigen::Matrix::Zero(), + .avg_reprojection_error = 0.0}; +#ifdef __linux__ +const std::string fileSuffix = ".avi"; +const std::string videoLocation = "/altfieldvideo"; +#else +const std::string fileSuffix = ".mp4"; +const std::string videoLocation = "/fieldvideo"; +#endif +TEST(Camera_CalibrationTest, OpenCV_Typical) { + int ret = cameracalibration::calibrate( + projectRootPath + "/testcalibration" + fileSuffix, cameraModel, 0.709f, + 0.551f, 12, 8, false); + cameracalibration::dumpJson(cameraModel, + calSavePath + "/cameracalibration.json"); + EXPECT_EQ(ret, 0); +} + +TEST(Camera_CalibrationTest, OpenCV_Atypical) { + int ret = cameracalibration::calibrate( + projectRootPath + videoLocation + "/long" + fileSuffix, cameraModel, + 0.709f, 0.551f, 12, 8, false); + EXPECT_EQ(ret, 1); +} + +TEST(Camera_CalibrationTest, MRcal_Typical) { + int ret = cameracalibration::calibrate( + projectRootPath + "/testcalibration" + fileSuffix, cameraModel, .709f, 12, + 8, 1080, 1920, false); + EXPECT_EQ(ret, 0); +} + +TEST(Camera_CalibrationTest, MRcal_Atypical) { + int ret = cameracalibration::calibrate( + projectRootPath + videoLocation + "/short" + fileSuffix, cameraModel, + 0.709f, 12, 8, 1080, 1920, false); + EXPECT_EQ(ret, 1); +} + +TEST(Field_CalibrationTest, Typical) { + int ret = fieldcalibration::calibrate( + projectRootPath + videoLocation, calSavePath, + calSavePath + "/cameracalibration.json", + projectRootPath + "/2024-crescendo.json", 3, false); + EXPECT_EQ(ret, 0); +} + +TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) { + int ret = fieldcalibration::calibrate( + projectRootPath + videoLocation, calSavePath, + projectRootPath + videoLocation + "/long" + fileSuffix, + projectRootPath + "/2024-crescendo.json", 3, false); + EXPECT_EQ(ret, 1); +} + +TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) { + int ret = fieldcalibration::calibrate( + projectRootPath + videoLocation, calSavePath, + calSavePath + "/cameracalibration.json", + calSavePath + "/cameracalibration.json", 3, false); + EXPECT_EQ(ret, 1); +} + +TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) { + int ret = fieldcalibration::calibrate( + projectRootPath + "", calSavePath, + calSavePath + "/cameracalibration.json", + projectRootPath + "/2024-crescendo.json", 3, false); + EXPECT_EQ(ret, 1); +} diff --git a/wpical/src/test/native/cpp/test_result_is_exact.cpp b/wpical/src/test/native/cpp/test_result_is_exact.cpp new file mode 100644 index 0000000000..8ff313fe59 --- /dev/null +++ b/wpical/src/test/native/cpp/test_result_is_exact.cpp @@ -0,0 +1,167 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace cv; + +struct cmpByFilename { + bool operator()(const std::string& a, const std::string& b) const { + auto a2 = std::stoi(a.substr(3, a.length() - 7)); + auto b2 = std::stoi(b.substr(3, b.length() - 7)); + // std::cout << a2 << " _ " << b2 << std::endl; + return a2 < b2; + } +}; + +std::vector calibrate(const std::string& fname, cv::Size boardSize, + cv::Size imagerSize) { + std::ifstream file(fname); + if (!file.is_open()) { + std::cerr << "Unable to open file corners.vnl" << std::endl; + return {}; + } + + std::string line; + std::map> points; + while (std::getline(file, line)) { + if (line.starts_with("#")) { + continue; + } + + if (line.empty()) { + continue; + } + + try { + std::istringstream iss(line); + std::string filename, x_str, y_str, level_str; + + iss >> filename >> x_str >> y_str >> level_str; + using std::stod; + + // From calibration.py: + // if weight_column_kind == 'level': the 4th column is a decimation level + // of the + // detected corner. If we needed to cut down the image resolution to + // detect a corner, its coordinates are known less precisely, and we use + // that information to weight the errors appropriately later. We set the + // output weight = 1/2^level. If the 4th column is '-' or <0, the given + // point was not detected, and should be ignored: we set weight = -1 + + double level; + if (level_str == "-") { + level = -1; + } else { + level = stod(level_str); + } + double weight; + if (level < 0) { + weight = -1; + } else { + weight = std::pow(0.5, level); + } + points[filename].push_back( + mrcal_point3_t{stod(x_str), stod(y_str), weight}); + // std::printf("put %s\n", *name); + } catch (std::exception const& e) { + std::perror(e.what()); + } + } + + file.close(); + + auto start = std::chrono::steady_clock::now(); + + std::vector observations_board; + std::vector frames_rt_toref; + // Pre-allocate worst case + size_t total_points = boardSize.width * boardSize.height * points.size(); + observations_board.reserve(total_points); + frames_rt_toref.reserve(points.size()); + + std::chrono::steady_clock::time_point begin = + std::chrono::steady_clock::now(); + + for (const auto& [key, value] : points) { + if (value.size()) { + auto ret = getSeedPose(value.data(), boardSize, imagerSize, 0.03, 1200); + + // Append to the Big List of board corners/levels + observations_board.insert(observations_board.end(), value.begin(), + value.end()); + // And list of pose seeds + frames_rt_toref.push_back(ret); + } else { + std::printf("No points for %s\n", key.c_str()); + } + } + + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + std::cout << "Seed took: " + << std::chrono::duration_cast(end - + begin) + .count() + << "[ms]" << std::endl; + + auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize, + 0.030, imagerSize, 1200); + + auto dt = std::chrono::steady_clock::now() - start; + int dt_ms = dt.count(); + + auto& stats = *cal_result; + + double max_error = + *std::max_element(stats.residuals.begin(), stats.residuals.end()); + + if (1) { + std::printf("\n===============================\n\n"); + std::printf("RMS Reprojection Error: %.2f pixels\n", stats.rms_error); + std::printf("Worst residual (by measurement): %.1f pixels\n", max_error); + std::printf("Noutliers: %i of %zu (%.1f percent of the data)\n", + stats.Noutliers_board, total_points, + 100.0 * stats.Noutliers_board / total_points); + std::printf("calobject_warp: [%f, %f]\n", stats.calobject_warp.x2, + stats.calobject_warp.y2); + std::printf("dt, seeding + solve: %f ms\n", dt_ms / 1e6); + std::printf("Intrinsics [%zu]: ", stats.intrinsics.size()); + for (auto i : stats.intrinsics) + std::printf("%f ", i); + std::printf("\n"); + } + + return stats.intrinsics; +} + +const std::string projectRootPath = PROJECT_ROOT_PATH; + +TEST(MrcalResultExactlyMatchesTest, lifecam_1280) { + auto calculated_intrinsics{calibrate( + projectRootPath + "/lifecam_1280p_10x10.vnl", {10, 10}, {1280, 720})}; + + // ## generated with mrgingham --jobs 4 --gridn 10 + // /home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/*.png + // # generated on 2024-12-01 11:51:55 with /usr/bin/mrcal-calibrate-cameras + // --corners-cache corners.vnl --lensmodel LENSMODEL_OPENCV8 --focal 1200 + // --object-spacing 0.0254 --object-width-n 10 '*.png' + std::vector mrcal_cli_groundtruth_intrinsics{ + 1130.892767, 1129.149453, 609.9417222, 349.6457279, + 0.1489878273, -1.348622726, 0.002839630852, 0.001135629909, + 2.560627057, -0.03170208336, 0.0695788644, -0.09547554864}; + + for (int i = 0; i < 12; i++) { + EXPECT_NEAR(mrcal_cli_groundtruth_intrinsics[i], calculated_intrinsics[i], + 1e-6); + } +} diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java index 4d9ad2c06f..704c690f5f 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java @@ -23,7 +23,7 @@ class CommandDecoratorTest extends CommandTestBase { HAL.initialize(500, 0); SimHooks.pauseTiming(); try (CommandScheduler scheduler = new CommandScheduler()) { - Command timeout = new RunCommand(() -> {}).withTimeout(0.1); + Command timeout = Commands.idle().withTimeout(0.1); scheduler.schedule(timeout); scheduler.run(); @@ -44,7 +44,7 @@ class CommandDecoratorTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean finish = new AtomicBoolean(); - Command command = new RunCommand(() -> {}).until(finish::get); + Command command = Commands.idle().until(finish::get); scheduler.schedule(command); scheduler.run(); @@ -93,7 +93,7 @@ class CommandDecoratorTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean run = new AtomicBoolean(true); - Command command = new RunCommand(() -> {}).onlyWhile(run::get); + Command command = Commands.idle().onlyWhile(run::get); scheduler.schedule(command); scheduler.run(); @@ -140,7 +140,7 @@ class CommandDecoratorTest extends CommandTestBase { @Test void ignoringDisableTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - var command = new RunCommand(() -> {}).ignoringDisable(true); + var command = Commands.idle().ignoringDisable(true); setDSEnabled(false); @@ -157,7 +157,7 @@ class CommandDecoratorTest extends CommandTestBase { AtomicBoolean finished = new AtomicBoolean(); finished.set(false); - Command command = new InstantCommand().beforeStarting(() -> finished.set(true)); + Command command = Commands.none().beforeStarting(() -> finished.set(true)); scheduler.schedule(command); @@ -178,7 +178,7 @@ class CommandDecoratorTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean finished = new AtomicBoolean(false); - Command command = new InstantCommand().andThen(() -> finished.set(true)); + Command command = Commands.none().andThen(() -> finished.set(true)); scheduler.schedule(command); @@ -199,8 +199,8 @@ class CommandDecoratorTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean condition = new AtomicBoolean(false); - Command command1 = new InstantCommand(); - Command command2 = new InstantCommand(() -> condition.set(true)); + Command command1 = Commands.none(); + Command command2 = Commands.runOnce(() -> condition.set(true)); Command group = command1.andThen(command2); scheduler.schedule(group); @@ -222,9 +222,9 @@ class CommandDecoratorTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean finish = new AtomicBoolean(false); - Command dictator = new WaitUntilCommand(finish::get); - Command endsBefore = new InstantCommand(); - Command endsAfter = new WaitUntilCommand(() -> false); + Command dictator = Commands.waitUntil(finish::get); + Command endsBefore = Commands.none(); + Command endsAfter = Commands.idle(); Command group = dictator.deadlineFor(endsBefore, endsAfter); @@ -256,7 +256,7 @@ class CommandDecoratorTest extends CommandTestBase { return true; }); Command other = - new RunCommand( + Commands.run( () -> assertAll( () -> assertTrue(dictatorHasRun.get()), @@ -327,8 +327,8 @@ class CommandDecoratorTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicBoolean finish = new AtomicBoolean(false); - Command command1 = new WaitUntilCommand(finish::get); - Command command2 = new InstantCommand(); + Command command1 = Commands.waitUntil(finish::get); + Command command2 = Commands.none(); Command group = command1.alongWith(command2); @@ -360,7 +360,7 @@ class CommandDecoratorTest extends CommandTestBase { return true; }); Command command2 = - new RunCommand( + Commands.run( () -> assertAll( () -> assertTrue(firstHasRun.get()), () -> assertTrue(firstWasPolled.get()))); @@ -377,8 +377,8 @@ class CommandDecoratorTest extends CommandTestBase { @Test void raceWithTest() { try (CommandScheduler scheduler = new CommandScheduler()) { - Command command1 = new WaitUntilCommand(() -> false); - Command command2 = new InstantCommand(); + Command command1 = Commands.idle(); + Command command2 = Commands.none(); Command group = command1.raceWith(command2); @@ -405,7 +405,7 @@ class CommandDecoratorTest extends CommandTestBase { return true; }); Command command2 = - new RunCommand( + Commands.run( () -> { assertTrue(firstHasRun.get()); assertTrue(firstWasPolled.get()); @@ -426,7 +426,7 @@ class CommandDecoratorTest extends CommandTestBase { AtomicBoolean hasRun = new AtomicBoolean(false); AtomicBoolean unlessCondition = new AtomicBoolean(true); - Command command = new InstantCommand(() -> hasRun.set(true)).unless(unlessCondition::get); + Command command = Commands.runOnce(() -> hasRun.set(true)).unless(unlessCondition::get); scheduler.schedule(command); scheduler.run(); @@ -445,7 +445,7 @@ class CommandDecoratorTest extends CommandTestBase { AtomicBoolean hasRun = new AtomicBoolean(false); AtomicBoolean onlyIfCondition = new AtomicBoolean(false); - Command command = new InstantCommand(() -> hasRun.set(true)).onlyIf(onlyIfCondition::get); + Command command = Commands.runOnce(() -> hasRun.set(true)).onlyIf(onlyIfCondition::get); scheduler.schedule(command); scheduler.run(); @@ -531,7 +531,7 @@ class CommandDecoratorTest extends CommandTestBase { @Test void withNameTest() { - InstantCommand command = new InstantCommand(); + Command command = Commands.none(); String name = "Named"; Command named = command.withName(name); assertEquals(name, named.getName()); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java index 47657e19ed..45f017c388 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java @@ -45,7 +45,7 @@ class CommandRequirementsTest extends CommandTestBase { Subsystem requirement = new SubsystemBase() {}; Command notInterrupted = - new RunCommand(() -> {}, requirement) + Commands.idle(requirement) .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming); MockCommandHolder interrupterHolder = new MockCommandHolder(true, requirement); Command interrupter = interrupterHolder.getMock(); @@ -63,7 +63,7 @@ class CommandRequirementsTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { Subsystem system = new SubsystemBase() {}; - Command missingRequirement = new WaitUntilCommand(() -> false); + Command missingRequirement = Commands.idle(); assertThrows( IllegalArgumentException.class, diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java index 4659f0ec3d..7b9817f564 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java @@ -74,34 +74,26 @@ class ConditionalCommandTest extends CommandTestBase { arguments( "AllCancelSelf", InterruptionBehavior.kCancelSelf, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), (BooleanSupplier) () -> true), arguments( "AllCancelIncoming", InterruptionBehavior.kCancelIncoming, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), (BooleanSupplier) () -> true), arguments( "OneCancelSelfOneIncoming", InterruptionBehavior.kCancelSelf, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), (BooleanSupplier) () -> true), arguments( "OneCancelIncomingOneSelf", InterruptionBehavior.kCancelSelf, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), (BooleanSupplier) () -> true)); } @@ -122,26 +114,26 @@ class ConditionalCommandTest extends CommandTestBase { arguments( "AllFalse", false, - new WaitUntilCommand(() -> false).ignoringDisable(false), - new WaitUntilCommand(() -> false).ignoringDisable(false), + Commands.idle().ignoringDisable(false), + Commands.idle().ignoringDisable(false), (BooleanSupplier) () -> true), arguments( "AllTrue", true, - new WaitUntilCommand(() -> false).ignoringDisable(true), - new WaitUntilCommand(() -> false).ignoringDisable(true), + Commands.idle().ignoringDisable(true), + Commands.idle().ignoringDisable(true), (BooleanSupplier) () -> true), arguments( "OneTrueOneFalse", false, - new WaitUntilCommand(() -> false).ignoringDisable(true), - new WaitUntilCommand(() -> false).ignoringDisable(false), + Commands.idle().ignoringDisable(true), + Commands.idle().ignoringDisable(false), (BooleanSupplier) () -> true), arguments( "OneFalseOneTrue", false, - new WaitUntilCommand(() -> false).ignoringDisable(false), - new WaitUntilCommand(() -> false).ignoringDisable(true), + Commands.idle().ignoringDisable(false), + Commands.idle().ignoringDisable(true), (BooleanSupplier) () -> true)); } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java index 9fafc5905c..1b0e8e9141 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MultiCompositionTestBase.java @@ -27,39 +27,27 @@ abstract class MultiCompositionTestBase extends SingleComposi arguments( "AllCancelSelf", InterruptionBehavior.kCancelSelf, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf)), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf)), arguments( "AllCancelIncoming", InterruptionBehavior.kCancelIncoming, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming)), arguments( "TwoCancelSelfOneIncoming", InterruptionBehavior.kCancelSelf, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming)), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming)), arguments( "TwoCancelIncomingOneSelf", InterruptionBehavior.kCancelSelf, - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelIncoming), - new WaitUntilCommand(() -> false) - .withInterruptBehavior(InterruptionBehavior.kCancelSelf))); + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelIncoming), + Commands.idle().withInterruptBehavior(InterruptionBehavior.kCancelSelf))); } @MethodSource @@ -79,27 +67,27 @@ abstract class MultiCompositionTestBase extends SingleComposi arguments( "AllFalse", false, - new WaitUntilCommand(() -> false).ignoringDisable(false), - new WaitUntilCommand(() -> false).ignoringDisable(false), - new WaitUntilCommand(() -> false).ignoringDisable(false)), + Commands.idle().ignoringDisable(false), + Commands.idle().ignoringDisable(false), + Commands.idle().ignoringDisable(false)), arguments( "AllTrue", true, - new WaitUntilCommand(() -> false).ignoringDisable(true), - new WaitUntilCommand(() -> false).ignoringDisable(true), - new WaitUntilCommand(() -> false).ignoringDisable(true)), + Commands.idle().ignoringDisable(true), + Commands.idle().ignoringDisable(true), + Commands.idle().ignoringDisable(true)), arguments( "TwoTrueOneFalse", false, - new WaitUntilCommand(() -> false).ignoringDisable(true), - new WaitUntilCommand(() -> false).ignoringDisable(true), - new WaitUntilCommand(() -> false).ignoringDisable(false)), + Commands.idle().ignoringDisable(true), + Commands.idle().ignoringDisable(true), + Commands.idle().ignoringDisable(false)), arguments( "TwoFalseOneTrue", false, - new WaitUntilCommand(() -> false).ignoringDisable(false), - new WaitUntilCommand(() -> false).ignoringDisable(false), - new WaitUntilCommand(() -> false).ignoringDisable(true))); + Commands.idle().ignoringDisable(false), + Commands.idle().ignoringDisable(false), + Commands.idle().ignoringDisable(true))); } @MethodSource @@ -115,8 +103,8 @@ abstract class MultiCompositionTestBase extends SingleComposi } static Stream composeDuplicates() { - Command a = new InstantCommand(() -> {}); - Command b = new InstantCommand(() -> {}); + Command a = Commands.none(); + Command b = Commands.none(); return Stream.of( arguments("AA", new Command[] {a, a}), arguments("ABA", new Command[] {a, b, a}), diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java index 206d801015..b3d7dfb556 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java @@ -127,15 +127,15 @@ class ParallelDeadlineGroupTest extends MultiCompositionTestBase {}); + Command a = Commands.none(); ParallelDeadlineGroup group = new ParallelDeadlineGroup(a); assertDoesNotThrow(() -> group.setDeadline(a)); } @Test void parallelDeadlineSetDeadlineDuplicateTest() { - Command a = new InstantCommand(() -> {}); - Command b = new InstantCommand(() -> {}); + Command a = Commands.none(); + Command b = Commands.none(); ParallelDeadlineGroup group = new ParallelDeadlineGroup(a, b); assertThrows(IllegalArgumentException.class, () -> group.setDeadline(b)); } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java index ea780c6216..bd44a128b8 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java @@ -140,7 +140,7 @@ class ParallelRaceGroupTest extends MultiCompositionTestBase MockCommandHolder command3Holder = new MockCommandHolder(true); Command command3 = command3Holder.getMock(); - Command group1 = new SequentialCommandGroup(command1, command2); + Command group1 = Commands.sequence(command1, command2); assertNotNull(group1); assertNotNull(command3); Command group2 = new ParallelRaceGroup(group1, command3); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyCommandTest.java index c4b43a609b..2eae20bbc8 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyCommandTest.java @@ -31,7 +31,7 @@ class ProxyCommandTest extends CommandTestBase { try (CommandScheduler scheduler = CommandScheduler.getInstance()) { AtomicBoolean cond = new AtomicBoolean(); - WaitUntilCommand command = new WaitUntilCommand(cond::get); + Command command = Commands.waitUntil(cond::get); ProxyCommand scheduleCommand = new ProxyCommand(command); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java index 9014bc7b96..875b2e5ea9 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java @@ -30,13 +30,12 @@ class ScheduleCommandTest extends CommandTestBase { @Test void scheduleCommandDuringRunTest() { try (CommandScheduler scheduler = CommandScheduler.getInstance()) { - InstantCommand toSchedule = new InstantCommand(); + Command toSchedule = Commands.none(); ScheduleCommand scheduleCommand = new ScheduleCommand(toSchedule); - SequentialCommandGroup group = - new SequentialCommandGroup(new InstantCommand(), scheduleCommand); + Command group = Commands.sequence(Commands.none(), scheduleCommand); scheduler.schedule(group); - scheduler.schedule(new RunCommand(() -> {})); + scheduler.schedule(Commands.idle()); scheduler.run(); assertDoesNotThrow(scheduler::run); } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java index 1e0b33332e..0a6eb3df5c 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java @@ -23,7 +23,7 @@ class SchedulerTest extends CommandTestBase { scheduler.onCommandExecute(command -> counter.incrementAndGet()); scheduler.onCommandFinish(command -> counter.incrementAndGet()); - scheduler.schedule(new InstantCommand()); + scheduler.schedule(Commands.none()); scheduler.run(); assertEquals(counter.get(), 3); @@ -37,7 +37,7 @@ class SchedulerTest extends CommandTestBase { scheduler.onCommandInterrupt(command -> counter.incrementAndGet()); - Command command = new WaitCommand(10); + Command command = Commands.idle(); scheduler.schedule(command); scheduler.cancel(command); @@ -162,8 +162,8 @@ class SchedulerTest extends CommandTestBase { scheduler.onCommandInterrupt(command -> counter.incrementAndGet()); scheduler.onCommandInterrupt((command, interruptor) -> assertFalse(interruptor.isPresent())); - Command command = new WaitCommand(10); - Command command2 = new WaitCommand(10); + Command command = Commands.idle(); + Command command2 = Commands.idle(); scheduler.schedule(command); scheduler.schedule(command2); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java index a253740c02..06f4023f6b 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulingRecursionTest.java @@ -48,7 +48,7 @@ class SchedulingRecursionTest extends CommandTestBase { return interruptionBehavior; } }; - Command other = new RunCommand(() -> hasOtherRun.set(true), requirement); + Command other = requirement.run(() -> hasOtherRun.set(true)); assertDoesNotThrow( () -> { @@ -87,7 +87,7 @@ class SchedulingRecursionTest extends CommandTestBase { return interruptionBehavior; } }; - Command other = new RunCommand(() -> hasOtherRun.set(true), requirement); + Command other = requirement.run(() -> hasOtherRun.set(true)); assertDoesNotThrow( () -> { @@ -132,7 +132,7 @@ class SchedulingRecursionTest extends CommandTestBase { return interruptionBehavior; } }; - Command other = new RunCommand(() -> hasOtherRun.set(true), requirement); + Command other = requirement.run(() -> hasOtherRun.set(true)); scheduler.setDefaultCommand(requirement, other); assertDoesNotThrow( @@ -173,7 +173,7 @@ class SchedulingRecursionTest extends CommandTestBase { void cancelFromInterruptAction() { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicInteger counter = new AtomicInteger(); - Command selfCancels = new RunCommand(() -> {}); + Command selfCancels = Commands.idle(); scheduler.onCommandInterrupt( cmd -> { counter.incrementAndGet(); @@ -329,7 +329,7 @@ class SchedulingRecursionTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicInteger counter = new AtomicInteger(); Subsystem requirement = new SubsystemBase() {}; - InstantCommand other = new InstantCommand(() -> {}, requirement); + Command other = requirement.runOnce(() -> {}); FunctionalCommand selfCancels = new FunctionalCommand( () -> {}, @@ -354,7 +354,7 @@ class SchedulingRecursionTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicInteger counter = new AtomicInteger(); Subsystem requirement = new SubsystemBase() {}; - InstantCommand other = new InstantCommand(() -> {}, requirement); + Command other = requirement.runOnce(() -> {}); FunctionalCommand selfCancels = new FunctionalCommand( () -> {}, @@ -380,8 +380,8 @@ class SchedulingRecursionTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicInteger counter = new AtomicInteger(); Subsystem requirement = new Subsystem() {}; - InstantCommand other = new InstantCommand(() -> {}, requirement); - InstantCommand selfCancels = new InstantCommand(() -> {}, requirement); + Command other = requirement.runOnce(() -> {}); + Command selfCancels = requirement.runOnce(() -> {}); scheduler.onCommandInterrupt( cmd -> { counter.incrementAndGet(); @@ -402,8 +402,7 @@ class SchedulingRecursionTest extends CommandTestBase { try (CommandScheduler scheduler = new CommandScheduler()) { AtomicInteger counter = new AtomicInteger(); Subsystem requirement = new SubsystemBase() {}; - Command other = - new InstantCommand(() -> {}, requirement).withInterruptBehavior(interruptionBehavior); + Command other = requirement.runOnce(() -> {}).withInterruptBehavior(interruptionBehavior); FunctionalCommand defaultCommand = new FunctionalCommand( () -> { @@ -438,7 +437,7 @@ class SchedulingRecursionTest extends CommandTestBase { interrupted -> counter.incrementAndGet(), () -> false, requirement); - Command other = new InstantCommand(() -> {}, requirement); + Command other = requirement.runOnce(() -> {}); Command cancelDefaultCommand = new FunctionalCommand( () -> {}, diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java index e364468186..f6f551bd6e 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java @@ -97,7 +97,7 @@ class SelectCommandTest extends MultiCompositionTestBase> () -> "one"); scheduler.schedule(selectCommand); - scheduler.schedule(new InstantCommand(() -> {}, system3)); + scheduler.schedule(system3.runOnce(() -> {})); assertFalse(scheduler.isScheduled(selectCommand)); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java index a063a1de63..5f6fe9e861 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SingleCompositionTestBase.java @@ -18,37 +18,34 @@ public abstract class SingleCompositionTestBase extends Comma @EnumSource(Command.InterruptionBehavior.class) @ParameterizedTest void interruptible(Command.InterruptionBehavior interruptionBehavior) { - var command = - composeSingle( - new WaitUntilCommand(() -> false).withInterruptBehavior(interruptionBehavior)); + var command = composeSingle(Commands.idle().withInterruptBehavior(interruptionBehavior)); assertEquals(interruptionBehavior, command.getInterruptionBehavior()); } @ValueSource(booleans = {true, false}) @ParameterizedTest void runWhenDisabled(boolean runsWhenDisabled) { - var command = - composeSingle(new WaitUntilCommand(() -> false).ignoringDisable(runsWhenDisabled)); + var command = composeSingle(Commands.idle().ignoringDisable(runsWhenDisabled)); assertEquals(runsWhenDisabled, command.runsWhenDisabled()); } @Test void commandInOtherCompositionTest() { - var command = new InstantCommand(); + var command = Commands.none(); new WrapperCommand(command) {}; assertThrows(IllegalArgumentException.class, () -> composeSingle(command)); } @Test void commandInMultipleCompositionsTest() { - var command = new InstantCommand(); + var command = Commands.none(); composeSingle(command); assertThrows(IllegalArgumentException.class, () -> composeSingle(command)); } @Test void composeThenScheduleTest() { - var command = new InstantCommand(); + var command = Commands.none(); composeSingle(command); assertThrows( IllegalArgumentException.class, () -> CommandScheduler.getInstance().schedule(command)); @@ -56,7 +53,7 @@ public abstract class SingleCompositionTestBase extends Comma @Test void scheduleThenComposeTest() { - var command = new RunCommand(() -> {}); + var command = Commands.idle(); CommandScheduler.getInstance().schedule(command); assertThrows(IllegalArgumentException.class, () -> composeSingle(command)); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp index a0b7726f5d..a8e057761f 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp @@ -8,6 +8,7 @@ #include #include "CommandTestBase.h" +#include "frc2/command/Commands.h" #include "frc2/command/FunctionalCommand.h" #include "frc2/command/InstantCommand.h" #include "frc2/command/RunCommand.h" @@ -21,7 +22,7 @@ TEST_F(CommandDecoratorTest, WithTimeout) { frc::sim::PauseTiming(); - auto command = RunCommand([] {}, {}).WithTimeout(100_ms); + auto command = cmd::Idle().WithTimeout(100_ms); scheduler.Schedule(command); scheduler.Run(); @@ -42,7 +43,7 @@ TEST_F(CommandDecoratorTest, Until) { bool finish = false; - auto command = RunCommand([] {}, {}).Until([&finish] { return finish; }); + auto command = cmd::Idle().Until([&finish] { return finish; }); scheduler.Schedule(command); scheduler.Run(); @@ -85,7 +86,7 @@ TEST_F(CommandDecoratorTest, OnlyWhile) { bool run = true; - auto command = RunCommand([] {}, {}).OnlyWhile([&run] { return run; }); + auto command = cmd::Idle().OnlyWhile([&run] { return run; }); scheduler.Schedule(command); scheduler.Run(); @@ -126,7 +127,7 @@ TEST_F(CommandDecoratorTest, OnlyWhileOrder) { TEST_F(CommandDecoratorTest, IgnoringDisable) { CommandScheduler scheduler = GetScheduler(); - auto command = RunCommand([] {}, {}).IgnoringDisable(true); + auto command = cmd::Idle().IgnoringDisable(true); SetDSEnabled(false); @@ -141,8 +142,7 @@ TEST_F(CommandDecoratorTest, BeforeStarting) { bool finished = false; - auto command = InstantCommand([] {}, {}).BeforeStarting( - [&finished] { finished = true; }); + auto command = cmd::None().BeforeStarting([&finished] { finished = true; }); scheduler.Schedule(command); @@ -162,8 +162,7 @@ TEST_F(CommandDecoratorTest, AndThenLambda) { bool finished = false; - auto command = - InstantCommand([] {}, {}).AndThen([&finished] { finished = true; }); + auto command = cmd::None().AndThen([&finished] { finished = true; }); scheduler.Schedule(command); @@ -183,9 +182,9 @@ TEST_F(CommandDecoratorTest, AndThen) { bool finished = false; - auto command1 = InstantCommand(); - auto command2 = InstantCommand([&finished] { finished = true; }); - auto group = std::move(command1).AndThen(std::move(command2).ToPtr()); + auto command1 = cmd::None(); + auto command2 = cmd::RunOnce([&finished] { finished = true; }); + auto group = std::move(command1).AndThen(std::move(command2)); scheduler.Schedule(group); @@ -205,10 +204,10 @@ TEST_F(CommandDecoratorTest, DeadlineFor) { bool finish = false; - auto dictator = WaitUntilCommand([&finish] { return finish; }); - auto endsAfter = WaitUntilCommand([] { return false; }); + auto dictator = cmd::WaitUntil([&finish] { return finish; }); + auto endsAfter = cmd::Idle(); - auto group = std::move(dictator).DeadlineFor(std::move(endsAfter).ToPtr()); + auto group = std::move(dictator).DeadlineFor(std::move(endsAfter)); scheduler.Schedule(group); scheduler.Run(); @@ -247,10 +246,10 @@ TEST_F(CommandDecoratorTest, AlongWith) { bool finish = false; - auto command1 = WaitUntilCommand([&finish] { return finish; }); - auto command2 = InstantCommand(); + auto command1 = cmd::WaitUntil([&finish] { return finish; }); + auto command2 = cmd::None(); - auto group = std::move(command1).AlongWith(std::move(command2).ToPtr()); + auto group = std::move(command1).AlongWith(std::move(command2)); scheduler.Schedule(group); scheduler.Run(); @@ -266,10 +265,10 @@ TEST_F(CommandDecoratorTest, AlongWith) { TEST_F(CommandDecoratorTest, RaceWith) { CommandScheduler scheduler = GetScheduler(); - auto command1 = WaitUntilCommand([] { return false; }); - auto command2 = InstantCommand(); + auto command1 = cmd::Idle(); + auto command2 = cmd::None(); - auto group = std::move(command1).RaceWith(std::move(command2).ToPtr()); + auto group = std::move(command1).RaceWith(std::move(command2)); scheduler.Schedule(group); scheduler.Run(); @@ -290,12 +289,12 @@ TEST_F(CommandDecoratorTest, DeadlineForOrder) { dictatorWasPolled = true; return true; }); - auto other = RunCommand([&dictatorHasRun, &dictatorWasPolled] { + auto other = cmd::Run([&dictatorHasRun, &dictatorWasPolled] { EXPECT_TRUE(dictatorHasRun); EXPECT_TRUE(dictatorWasPolled); }); - auto group = std::move(dictator).DeadlineFor(std::move(other).ToPtr()); + auto group = std::move(dictator).DeadlineFor(std::move(other)); scheduler.Schedule(group); scheduler.Run(); @@ -343,12 +342,12 @@ TEST_F(CommandDecoratorTest, AlongWithOrder) { firstWasPolled = true; return true; }); - auto command2 = RunCommand([&firstHasRun, &firstWasPolled] { + auto command2 = cmd::Run([&firstHasRun, &firstWasPolled] { EXPECT_TRUE(firstHasRun); EXPECT_TRUE(firstWasPolled); }); - auto group = std::move(command1).AlongWith(std::move(command2).ToPtr()); + auto group = std::move(command1).AlongWith(std::move(command2)); scheduler.Schedule(group); scheduler.Run(); @@ -369,12 +368,12 @@ TEST_F(CommandDecoratorTest, RaceWithOrder) { firstWasPolled = true; return true; }); - auto command2 = RunCommand([&firstHasRun, &firstWasPolled] { + auto command2 = cmd::Run([&firstHasRun, &firstWasPolled] { EXPECT_TRUE(firstHasRun); EXPECT_TRUE(firstWasPolled); }); - auto group = std::move(command1).RaceWith(std::move(command2).ToPtr()); + auto group = std::move(command1).RaceWith(std::move(command2)); scheduler.Schedule(group); scheduler.Run(); @@ -389,8 +388,10 @@ TEST_F(CommandDecoratorTest, Unless) { bool hasRun = false; bool unlessCondition = true; - auto command = InstantCommand([&hasRun] { hasRun = true; }, {}) - .Unless([&unlessCondition] { return unlessCondition; }); + auto command = + cmd::RunOnce([&hasRun] { hasRun = true; }, {}).Unless([&unlessCondition] { + return unlessCondition; + }); scheduler.Schedule(command); scheduler.Run(); @@ -408,8 +409,10 @@ TEST_F(CommandDecoratorTest, OnlyIf) { bool hasRun = false; bool onlyIfCondition = false; - auto command = InstantCommand([&hasRun] { hasRun = true; }, {}) - .OnlyIf([&onlyIfCondition] { return onlyIfCondition; }); + auto command = + cmd::RunOnce([&hasRun] { hasRun = true; }, {}).OnlyIf([&onlyIfCondition] { + return onlyIfCondition; + }); scheduler.Schedule(command); scheduler.Run(); @@ -481,8 +484,8 @@ TEST_F(CommandDecoratorTest, HandleInterrupt) { } TEST_F(CommandDecoratorTest, WithName) { - InstantCommand command; + auto command = cmd::None(); std::string name{"Named"}; - CommandPtr named = std::move(command).WithName(name); + auto named = std::move(command).WithName(name); EXPECT_EQ(name, named.get()->GetName()); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CompositionTestBase.h b/wpilibNewCommands/src/test/native/cpp/frc2/command/CompositionTestBase.h index 58fbcc393b..4a641180fb 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CompositionTestBase.h +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CompositionTestBase.h @@ -23,15 +23,13 @@ TYPED_TEST_SUITE_P(SingleCompositionRunsWhenDisabledTest); TYPED_TEST_P(SingleCompositionRunsWhenDisabledTest, True) { auto param = true; - TypeParam command = TypeParam( - cmd::WaitUntil([] { return false; }).IgnoringDisable(param).Unwrap()); + TypeParam command = TypeParam(cmd::Idle().IgnoringDisable(param).Unwrap()); EXPECT_EQ(param, command.RunsWhenDisabled()); } TYPED_TEST_P(SingleCompositionRunsWhenDisabledTest, False) { auto param = false; - TypeParam command = TypeParam( - cmd::WaitUntil([] { return false; }).IgnoringDisable(param).Unwrap()); + TypeParam command = TypeParam(cmd::Idle().IgnoringDisable(param).Unwrap()); EXPECT_EQ(param, command.RunsWhenDisabled()); } @@ -44,17 +42,15 @@ TYPED_TEST_SUITE_P(SingleCompositionInterruptibilityTest); TYPED_TEST_P(SingleCompositionInterruptibilityTest, CancelSelf) { auto param = Command::InterruptionBehavior::kCancelSelf; - TypeParam command = TypeParam(cmd::WaitUntil([] { return false; }) - .WithInterruptBehavior(param) - .Unwrap()); + TypeParam command = + TypeParam(cmd::Idle().WithInterruptBehavior(param).Unwrap()); EXPECT_EQ(param, command.GetInterruptionBehavior()); } TYPED_TEST_P(SingleCompositionInterruptibilityTest, CancelIncoming) { auto param = Command::InterruptionBehavior::kCancelIncoming; - TypeParam command = TypeParam(cmd::WaitUntil([] { return false; }) - .WithInterruptBehavior(param) - .Unwrap()); + TypeParam command = + TypeParam(cmd::Idle().WithInterruptBehavior(param).Unwrap()); EXPECT_EQ(param, command.GetInterruptionBehavior()); } @@ -77,47 +73,43 @@ TYPED_TEST_SUITE_P(MultiCompositionRunsWhenDisabledTest); TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, OneTrue) { auto param = true; - TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { return false; }).IgnoringDisable(param)))); + TypeParam command = TypeParam(CommandPtr::UnwrapVector( + cmd::impl::MakeVector(cmd::Idle().IgnoringDisable(param)))); EXPECT_EQ(param, command.RunsWhenDisabled()); } TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, OneFalse) { auto param = false; - TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { return false; }).IgnoringDisable(param)))); + TypeParam command = TypeParam(CommandPtr::UnwrapVector( + cmd::impl::MakeVector(cmd::Idle().IgnoringDisable(param)))); EXPECT_EQ(param, command.RunsWhenDisabled()); } TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, AllTrue) { TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - cmd::WaitUntil([] { return false; }).IgnoringDisable(true)))); + cmd::Idle().IgnoringDisable(true), cmd::Idle().IgnoringDisable(true), + cmd::Idle().IgnoringDisable(true)))); EXPECT_EQ(true, command.RunsWhenDisabled()); } TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, AllFalse) { TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - cmd::WaitUntil([] { return false; }).IgnoringDisable(false)))); + cmd::Idle().IgnoringDisable(false), cmd::Idle().IgnoringDisable(false), + cmd::Idle().IgnoringDisable(false)))); EXPECT_EQ(false, command.RunsWhenDisabled()); } TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, TwoTrueOneFalse) { TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - cmd::WaitUntil([] { return false; }).IgnoringDisable(false)))); + cmd::Idle().IgnoringDisable(true), cmd::Idle().IgnoringDisable(true), + cmd::Idle().IgnoringDisable(false)))); EXPECT_EQ(false, command.RunsWhenDisabled()); } TYPED_TEST_P(MultiCompositionRunsWhenDisabledTest, TwoFalseOneTrue) { TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - cmd::WaitUntil([] { return false; }).IgnoringDisable(true)))); + cmd::Idle().IgnoringDisable(false), cmd::Idle().IgnoringDisable(false), + cmd::Idle().IgnoringDisable(true)))); EXPECT_EQ(false, command.RunsWhenDisabled()); } @@ -132,61 +124,49 @@ class MultiCompositionInterruptibilityTest TYPED_TEST_SUITE_P(MultiCompositionInterruptibilityTest); TYPED_TEST_P(MultiCompositionInterruptibilityTest, AllCancelSelf) { - TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf)))); + TypeParam command = TypeParam(CommandPtr::UnwrapVector( + cmd::impl::MakeVector(cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf)))); EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf, command.GetInterruptionBehavior()); } TYPED_TEST_P(MultiCompositionInterruptibilityTest, AllCancelIncoming) { TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - cmd::WaitUntil([] { return false; }) - .WithInterruptBehavior( - Command::InterruptionBehavior::kCancelIncoming)))); + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming)))); EXPECT_EQ(Command::InterruptionBehavior::kCancelIncoming, command.GetInterruptionBehavior()); } TYPED_TEST_P(MultiCompositionInterruptibilityTest, TwoCancelSelfOneIncoming) { TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - cmd::WaitUntil([] { return false; }) - .WithInterruptBehavior( - Command::InterruptionBehavior::kCancelIncoming)))); + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming)))); EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf, command.GetInterruptionBehavior()); } TYPED_TEST_P(MultiCompositionInterruptibilityTest, TwoCancelIncomingOneSelf) { - TypeParam command = TypeParam(CommandPtr::UnwrapVector(cmd::impl::MakeVector( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf)))); + TypeParam command = TypeParam(CommandPtr::UnwrapVector( + cmd::impl::MakeVector(cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf)))); EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf, command.GetInterruptionBehavior()); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp index 7896a1aa32..fefb75d0e7 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp @@ -56,85 +56,72 @@ TEST_F(ConditionalCommandTest, ConditionalCommandRequirement) { } TEST_F(ConditionalCommandTest, AllTrue) { - CommandPtr command = - cmd::Either(cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - [] { return true; }); + auto command = + cmd::Either(cmd::Idle().IgnoringDisable(true), + cmd::Idle().IgnoringDisable(true), [] { return true; }); EXPECT_EQ(true, command.get()->RunsWhenDisabled()); } TEST_F(ConditionalCommandTest, AllFalse) { - CommandPtr command = - cmd::Either(cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - [] { return true; }); + auto command = + cmd::Either(cmd::Idle().IgnoringDisable(false), + cmd::Idle().IgnoringDisable(false), [] { return true; }); EXPECT_EQ(false, command.get()->RunsWhenDisabled()); } TEST_F(ConditionalCommandTest, OneTrueOneFalse) { - CommandPtr command = - cmd::Either(cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - [] { return true; }); + auto command = + cmd::Either(cmd::Idle().IgnoringDisable(true), + cmd::Idle().IgnoringDisable(false), [] { return true; }); EXPECT_EQ(false, command.get()->RunsWhenDisabled()); } TEST_F(ConditionalCommandTest, TwoFalseOneTrue) { - CommandPtr command = - cmd::Either(cmd::WaitUntil([] { return false; }).IgnoringDisable(false), - cmd::WaitUntil([] { return false; }).IgnoringDisable(true), - [] { return true; }); + auto command = + cmd::Either(cmd::Idle().IgnoringDisable(false), + cmd::Idle().IgnoringDisable(true), [] { return true; }); EXPECT_EQ(false, command.get()->RunsWhenDisabled()); } TEST_F(ConditionalCommandTest, AllCancelSelf) { - CommandPtr command = cmd::Either( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - [] { return true; }); + auto command = cmd::Either(cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + [] { return true; }); EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf, command.get()->GetInterruptionBehavior()); } TEST_F(ConditionalCommandTest, AllCancelIncoming) { - CommandPtr command = cmd::Either( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - [] { return false; }); + auto command = + cmd::Either(cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + [] { return false; }); EXPECT_EQ(Command::InterruptionBehavior::kCancelIncoming, command.get()->GetInterruptionBehavior()); } TEST_F(ConditionalCommandTest, OneCancelSelfOneIncoming) { - CommandPtr command = cmd::Either( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - [] { return false; }); + auto command = + cmd::Either(cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + [] { return false; }); EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf, command.get()->GetInterruptionBehavior()); } TEST_F(ConditionalCommandTest, OneCancelIncomingOneSelf) { - CommandPtr command = cmd::Either( - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelIncoming), - cmd::WaitUntil([] { - return false; - }).WithInterruptBehavior(Command::InterruptionBehavior::kCancelSelf), - [] { return false; }); + auto command = + cmd::Either(cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelIncoming), + cmd::Idle().WithInterruptBehavior( + Command::InterruptionBehavior::kCancelSelf), + [] { return false; }); EXPECT_EQ(Command::InterruptionBehavior::kCancelSelf, command.get()->GetInterruptionBehavior()); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp index a024a2f195..25acb7f979 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp @@ -5,6 +5,7 @@ #include #include "CommandTestBase.h" +#include "frc2/command/Commands.h" #include "frc2/command/RunCommand.h" using namespace frc2; @@ -15,9 +16,9 @@ TEST_F(DefaultCommandTest, DefaultCommandSchedule) { TestSubsystem subsystem; - RunCommand command1([] {}, {&subsystem}); + auto command = cmd::Idle({&subsystem}); - scheduler.SetDefaultCommand(&subsystem, std::move(command1)); + scheduler.SetDefaultCommand(&subsystem, std::move(command)); auto handle = scheduler.GetDefaultCommand(&subsystem); scheduler.Run(); @@ -29,18 +30,18 @@ TEST_F(DefaultCommandTest, DefaultCommandInterruptResume) { TestSubsystem subsystem; - RunCommand command1([] {}, {&subsystem}); - RunCommand command2([] {}, {&subsystem}); + auto command1 = cmd::Idle({&subsystem}); + auto command2 = cmd::Idle({&subsystem}); scheduler.SetDefaultCommand(&subsystem, std::move(command1)); auto handle = scheduler.GetDefaultCommand(&subsystem); scheduler.Run(); - scheduler.Schedule(&command2); + scheduler.Schedule(command2); - EXPECT_TRUE(scheduler.IsScheduled(&command2)); + EXPECT_TRUE(scheduler.IsScheduled(command2)); EXPECT_FALSE(scheduler.IsScheduled(handle)); - scheduler.Cancel(&command2); + scheduler.Cancel(command2); scheduler.Run(); EXPECT_TRUE(scheduler.IsScheduled(handle)); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp index 90eab85223..9b01721e45 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp @@ -3,6 +3,7 @@ // the WPILib BSD license file in the root directory of this project. #include "CommandTestBase.h" +#include "frc2/command/Commands.h" #include "frc2/command/InstantCommand.h" using namespace frc2; @@ -13,10 +14,10 @@ TEST_F(InstantCommandTest, InstantCommandSchedule) { int counter = 0; - InstantCommand command([&counter] { counter++; }, {}); + auto command = cmd::RunOnce([&counter] { counter++; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); EXPECT_EQ(counter, 1); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp index 21457d024c..2f22a30a1e 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp @@ -78,9 +78,9 @@ TEST_F(ParallelCommandGroupTest, ParallelGroupInterrupt) { TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancel) { CommandScheduler scheduler = GetScheduler(); - ParallelCommandGroup group((InstantCommand(), InstantCommand())); + auto group = cmd::Parallel(cmd::None(), cmd::None()); - EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(group)); } TEST_F(ParallelCommandGroupTest, ParallelGroupCopy) { @@ -88,15 +88,15 @@ TEST_F(ParallelCommandGroupTest, ParallelGroupCopy) { bool finished = false; - WaitUntilCommand command([&finished] { return finished; }); + auto command = cmd::WaitUntil([&finished] { return finished; }); - ParallelCommandGroup group(command); - scheduler.Schedule(&group); + auto group = cmd::Parallel(std::move(command)); + scheduler.Schedule(group); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(group)); finished = true; scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } TEST_F(ParallelCommandGroupTest, ParallelGroupRequirement) { @@ -107,17 +107,17 @@ TEST_F(ParallelCommandGroupTest, ParallelGroupRequirement) { TestSubsystem requirement3; TestSubsystem requirement4; - InstantCommand command1([] {}, {&requirement1, &requirement2}); - InstantCommand command2([] {}, {&requirement3}); - InstantCommand command3([] {}, {&requirement3, &requirement4}); + auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2}); + auto command2 = cmd::RunOnce([] {}, {&requirement3}); + auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4}); - ParallelCommandGroup group(std::move(command1), std::move(command2)); + auto group = cmd::Parallel(std::move(command1), std::move(command2)); - scheduler.Schedule(&group); - scheduler.Schedule(&command3); + scheduler.Schedule(group); + scheduler.Schedule(command3); - EXPECT_TRUE(scheduler.IsScheduled(&command3)); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(command3)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(ParallelCommandGroupTest, diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp index d36b97ad6c..9da797ccad 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp @@ -95,9 +95,9 @@ TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterrupt) { TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancel) { CommandScheduler scheduler = GetScheduler(); - ParallelDeadlineGroup group{InstantCommand(), InstantCommand()}; + auto group = cmd::Deadline(cmd::None(), cmd::None()); - EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(group)); } TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopy) { @@ -105,15 +105,15 @@ TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopy) { bool finished = false; - WaitUntilCommand command([&finished] { return finished; }); + auto command = cmd::WaitUntil([&finished] { return finished; }); - ParallelDeadlineGroup group(command); - scheduler.Schedule(&group); + auto group = cmd::Deadline(std::move(command)); + scheduler.Schedule(group); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(group)); finished = true; scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirement) { @@ -124,17 +124,17 @@ TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirement) { TestSubsystem requirement3; TestSubsystem requirement4; - InstantCommand command1([] {}, {&requirement1, &requirement2}); - InstantCommand command2([] {}, {&requirement3}); - InstantCommand command3([] {}, {&requirement3, &requirement4}); + auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2}); + auto command2 = cmd::RunOnce([] {}, {&requirement3}); + auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4}); - ParallelDeadlineGroup group(std::move(command1), std::move(command2)); + auto group = cmd::Deadline(std::move(command1), std::move(command2)); - scheduler.Schedule(&group); - scheduler.Schedule(&command3); + scheduler.Schedule(group); + scheduler.Schedule(command3); - EXPECT_TRUE(scheduler.IsScheduled(&command3)); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(command3)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } class TestableDeadlineCommand : public ParallelDeadlineGroup { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp index 032c3c1dd3..491ba75acb 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp @@ -90,9 +90,9 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceInterrupt) { TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancel) { CommandScheduler scheduler = GetScheduler(); - ParallelRaceGroup group{InstantCommand(), InstantCommand()}; + auto group = cmd::Race(cmd::None(), cmd::None()); - EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group)); + EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(group)); } TEST_F(ParallelRaceGroupTest, ParallelRaceCopy) { @@ -100,15 +100,15 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceCopy) { bool finished = false; - WaitUntilCommand command([&finished] { return finished; }); + auto command = cmd::WaitUntil([&finished] { return finished; }); - ParallelRaceGroup group(command); - scheduler.Schedule(&group); + auto group = cmd::Race(std::move(command)); + scheduler.Schedule(group); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(group)); finished = true; scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } TEST_F(ParallelRaceGroupTest, RaceGroupRequirement) { @@ -119,17 +119,17 @@ TEST_F(ParallelRaceGroupTest, RaceGroupRequirement) { TestSubsystem requirement3; TestSubsystem requirement4; - InstantCommand command1([] {}, {&requirement1, &requirement2}); - InstantCommand command2([] {}, {&requirement3}); - InstantCommand command3([] {}, {&requirement3, &requirement4}); + auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2}); + auto command2 = cmd::RunOnce([] {}, {&requirement3}); + auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4}); - ParallelRaceGroup group(std::move(command1), std::move(command2)); + auto group = cmd::Race(std::move(command1), std::move(command2)); - scheduler.Schedule(&group); - scheduler.Schedule(&command3); + scheduler.Schedule(group); + scheduler.Schedule(command3); - EXPECT_TRUE(scheduler.IsScheduled(&command3)); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(command3)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnce) { @@ -139,21 +139,21 @@ TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnce) { bool finished2 = false; bool finished3 = false; - WaitUntilCommand command1([&finished1] { return finished1; }); - WaitUntilCommand command2([&finished2] { return finished2; }); - WaitUntilCommand command3([&finished3] { return finished3; }); + auto command1 = cmd::WaitUntil([&finished1] { return finished1; }); + auto command2 = cmd::WaitUntil([&finished2] { return finished2; }); + auto command3 = cmd::WaitUntil([&finished3] { return finished3; }); - SequentialCommandGroup group1(command1, command2); - ParallelRaceGroup group2(std::move(group1), command3); + auto group1 = cmd::Sequence(std::move(command1), std::move(command2)); + auto group2 = cmd::Race(std::move(group1), std::move(command3)); - scheduler.Schedule(&group2); + scheduler.Schedule(group2); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&group2)); + EXPECT_TRUE(scheduler.IsScheduled(group2)); finished1 = true; scheduler.Run(); finished2 = true; EXPECT_NO_FATAL_FAILURE(scheduler.Run()); - EXPECT_FALSE(scheduler.IsScheduled(&group2)); + EXPECT_FALSE(scheduler.IsScheduled(group2)); } TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwice) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp index 02575a30cc..77906f1e61 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include #include "CommandTestBase.h" @@ -13,15 +15,15 @@ class PrintCommandTest : public CommandTestBase {}; TEST_F(PrintCommandTest, PrintCommandSchedule) { CommandScheduler scheduler = GetScheduler(); - PrintCommand command("Test!"); + auto command = cmd::Print("Test!"); testing::internal::CaptureStdout(); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); EXPECT_TRUE(std::regex_search(testing::internal::GetCapturedStdout(), std::regex("Test!"))); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyCommandTest.cpp index 81ec9d1432..7abc56fa11 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyCommandTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include #include "CommandTestBase.h" @@ -52,8 +54,7 @@ TEST_F(ProxyCommandTest, OwningCommandSchedule) { bool scheduled = false; - CommandPtr command = - InstantCommand([&scheduled] { scheduled = true; }).AsProxy(); + auto command = cmd::RunOnce([&scheduled] { scheduled = true; }).AsProxy(); scheduler.Schedule(command); scheduler.Run(); @@ -66,8 +67,7 @@ TEST_F(ProxyCommandTest, OwningCommandEnd) { bool finished = false; - CommandPtr command = - WaitUntilCommand([&finished] { return finished; }).AsProxy(); + auto command = cmd::WaitUntil([&finished] { return finished; }).AsProxy(); scheduler.Schedule(command); scheduler.Run(); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp index 11d4adac76..07eaaf54c5 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include "CommandTestBase.h" #include "frc2/command/RunCommand.h" @@ -13,9 +15,9 @@ TEST_F(RunCommandTest, RunCommandSchedule) { int counter = 0; - RunCommand command([&counter] { counter++; }, {}); + auto command = cmd::Run([&counter] { counter++; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); scheduler.Run(); scheduler.Run(); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp index bc4756cccc..24ca0ecd5e 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include #include "CommandTestBase.h" @@ -15,7 +17,7 @@ class SchedulerTest : public CommandTestBase {}; TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) { CommandScheduler scheduler = GetScheduler(); - InstantCommand command; + auto command = cmd::None(); int counter = 0; @@ -23,7 +25,7 @@ TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) { scheduler.OnCommandExecute([&counter](const Command&) { counter++; }); scheduler.OnCommandFinish([&counter](const Command&) { counter++; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); EXPECT_EQ(counter, 3); @@ -32,15 +34,15 @@ TEST_F(SchedulerTest, SchedulerLambdaTestNoInterrupt) { TEST_F(SchedulerTest, SchedulerLambdaInterrupt) { CommandScheduler scheduler = GetScheduler(); - RunCommand command([] {}, {}); + auto command = cmd::Idle(); int counter = 0; scheduler.OnCommandInterrupt([&counter](const Command&) { counter++; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - scheduler.Cancel(&command); + scheduler.Cancel(command); EXPECT_EQ(counter, 1); } @@ -56,10 +58,10 @@ TEST_F(SchedulerTest, SchedulerLambdaInterruptNoCause) { counter++; }); - RunCommand command([] {}); + auto command = cmd::Idle(); - scheduler.Schedule(&command); - scheduler.Cancel(&command); + scheduler.Schedule(command); + scheduler.Cancel(command); EXPECT_EQ(1, counter); } @@ -70,7 +72,7 @@ TEST_F(SchedulerTest, SchedulerLambdaInterruptCause) { int counter = 0; TestSubsystem subsystem{}; - RunCommand command([] {}, {&subsystem}); + auto command = cmd::Idle({&subsystem}); InstantCommand interruptor([] {}, {&subsystem}); scheduler.OnCommandInterrupt( @@ -80,7 +82,7 @@ TEST_F(SchedulerTest, SchedulerLambdaInterruptCause) { counter++; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Schedule(&interruptor); EXPECT_EQ(1, counter); @@ -92,11 +94,11 @@ TEST_F(SchedulerTest, SchedulerLambdaInterruptCauseInRunLoop) { int counter = 0; TestSubsystem subsystem{}; - RunCommand command([] {}, {&subsystem}); + auto command = cmd::Idle({&subsystem}); InstantCommand interruptor([] {}, {&subsystem}); // This command will schedule interruptor in execute() inside the run loop - InstantCommand interruptorScheduler( - [&] { scheduler.Schedule(&interruptor); }); + auto interruptorScheduler = + cmd::RunOnce([&] { scheduler.Schedule(&interruptor); }); scheduler.OnCommandInterrupt( [&](const Command&, const std::optional& cause) { @@ -105,8 +107,8 @@ TEST_F(SchedulerTest, SchedulerLambdaInterruptCauseInRunLoop) { counter++; }); - scheduler.Schedule(&command); - scheduler.Schedule(&interruptorScheduler); + scheduler.Schedule(command); + scheduler.Schedule(interruptorScheduler); scheduler.Run(); @@ -142,8 +144,8 @@ TEST_F(SchedulerTest, UnregisterSubsystem) { TEST_F(SchedulerTest, SchedulerCancelAll) { CommandScheduler scheduler = GetScheduler(); - RunCommand command([] {}, {}); - RunCommand command2([] {}, {}); + auto command1 = cmd::Idle(); + auto command2 = cmd::Idle(); int counter = 0; @@ -153,8 +155,8 @@ TEST_F(SchedulerTest, SchedulerCancelAll) { EXPECT_FALSE(interruptor); }); - scheduler.Schedule(&command); - scheduler.Schedule(&command2); + scheduler.Schedule(command1); + scheduler.Schedule(command2); scheduler.Run(); scheduler.CancelAll(); @@ -166,10 +168,10 @@ TEST_F(SchedulerTest, ScheduleScheduledNoOp) { int counter = 0; - StartEndCommand command([&counter] { counter++; }, [] {}); + auto command = cmd::StartEnd([&counter] { counter++; }, [] {}); - scheduler.Schedule(&command); - scheduler.Schedule(&command); + scheduler.Schedule(command); + scheduler.Schedule(command); EXPECT_EQ(counter, 1); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp index 0b54c58951..650319d8cf 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulingRecursionTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include #include @@ -55,14 +57,14 @@ TEST_P(SchedulingRecursionTest, CancelFromInitialize) { TestSubsystem requirement; SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, GetParam()}; - RunCommand other{[&hasOtherRun] { hasOtherRun = true; }, {&requirement}}; + auto other = cmd::Run([&hasOtherRun] { hasOtherRun = true; }, {&requirement}); scheduler.Schedule(&selfCancels); scheduler.Run(); - scheduler.Schedule(&other); + scheduler.Schedule(other); EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); - EXPECT_TRUE(scheduler.IsScheduled(&other)); + EXPECT_TRUE(scheduler.IsScheduled(other)); EXPECT_EQ(1, counter); scheduler.Run(); EXPECT_TRUE(hasOtherRun); @@ -78,16 +80,16 @@ TEST_F(SchedulingRecursionTest, CancelFromInitializeAction) { [&counter](bool) { counter++; }, [] { return false; }, {&requirement}}; - RunCommand other{[&hasOtherRun] { hasOtherRun = true; }, {&requirement}}; + auto other = cmd::Run([&hasOtherRun] { hasOtherRun = true; }, {&requirement}); scheduler.OnCommandInitialize([&scheduler, &selfCancels](const Command&) { scheduler.Cancel(&selfCancels); }); scheduler.Schedule(&selfCancels); scheduler.Run(); - scheduler.Schedule(&other); + scheduler.Schedule(other); EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); - EXPECT_TRUE(scheduler.IsScheduled(&other)); + EXPECT_TRUE(scheduler.IsScheduled(other)); EXPECT_EQ(1, counter); scheduler.Run(); EXPECT_TRUE(hasOtherRun); @@ -101,7 +103,7 @@ TEST_P(SchedulingRecursionTest, TestSubsystem requirement; SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, GetParam()}; - RunCommand other{[&hasOtherRun] { hasOtherRun = true; }, {&requirement}}; + auto other = cmd::Run([&hasOtherRun] { hasOtherRun = true; }, {&requirement}); scheduler.SetDefaultCommand(&requirement, std::move(other)); scheduler.Schedule(&selfCancels); @@ -202,6 +204,7 @@ TEST_F(SchedulingRecursionTest, CancelFromEndLoop) { TEST_F(SchedulingRecursionTest, CancelFromEndLoopWhileInRunLoop) { CommandScheduler scheduler = GetScheduler(); int counter = 0; + EndCommand dCancelsAll([&](bool) { counter++; scheduler.CancelAll(); @@ -270,7 +273,6 @@ TEST_P(SchedulingRecursionTest, ScheduleFromEndCancel) { TestSubsystem requirement; SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, GetParam()}; - RunCommand other{[] {}, {&requirement}}; scheduler.Schedule(&selfCancels); EXPECT_NO_THROW({ scheduler.Cancel(&selfCancels); }); @@ -284,30 +286,30 @@ TEST_P(SchedulingRecursionTest, ScheduleFromEndInterrupt) { TestSubsystem requirement; SelfCancellingCommand selfCancels{&scheduler, counter, &requirement, GetParam()}; - RunCommand other{[] {}, {&requirement}}; + auto other = cmd::Idle({&requirement}); scheduler.Schedule(&selfCancels); - EXPECT_NO_THROW({ scheduler.Schedule(&other); }); + EXPECT_NO_THROW({ scheduler.Schedule(other); }); EXPECT_EQ(1, counter); EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); - EXPECT_TRUE(scheduler.IsScheduled(&other)); + EXPECT_TRUE(scheduler.IsScheduled(other)); } TEST_F(SchedulingRecursionTest, ScheduleFromEndInterruptAction) { CommandScheduler scheduler = GetScheduler(); int counter = 0; TestSubsystem requirement; - RunCommand selfCancels{[] {}, {&requirement}}; - RunCommand other{[] {}, {&requirement}}; + auto selfCancels = cmd::Idle({&requirement}); + auto other = cmd::Idle({&requirement}); scheduler.OnCommandInterrupt([&](const Command&) { counter++; - scheduler.Schedule(&other); + scheduler.Schedule(other); }); - scheduler.Schedule(&selfCancels); - EXPECT_NO_THROW({ scheduler.Schedule(&other); }); + scheduler.Schedule(selfCancels); + EXPECT_NO_THROW({ scheduler.Schedule(other); }); EXPECT_EQ(1, counter); - EXPECT_FALSE(scheduler.IsScheduled(&selfCancels)); - EXPECT_TRUE(scheduler.IsScheduled(&other)); + EXPECT_FALSE(scheduler.IsScheduled(selfCancels)); + EXPECT_TRUE(scheduler.IsScheduled(other)); } TEST_F(SchedulingRecursionTest, CancelDefaultCommandFromEnd) { @@ -319,11 +321,11 @@ TEST_F(SchedulingRecursionTest, CancelDefaultCommandFromEnd) { [&counter](bool) { counter++; }, [] { return false; }, {&requirement}}; - RunCommand other{[] {}, {&requirement}}; + auto other = cmd::Idle({&requirement}); FunctionalCommand cancelDefaultCommand{[] {}, [] {}, [&](bool) { counter++; - scheduler.Schedule(&other); + scheduler.Schedule(other); }, [] { return false; }}; @@ -336,7 +338,7 @@ TEST_F(SchedulingRecursionTest, CancelDefaultCommandFromEnd) { }); EXPECT_EQ(2, counter); EXPECT_FALSE(scheduler.IsScheduled(&defaultCommand)); - EXPECT_TRUE(scheduler.IsScheduled(&other)); + EXPECT_TRUE(scheduler.IsScheduled(other)); } TEST_F(SchedulingRecursionTest, CancelNextCommandFromCommand) { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp index dc882a0ed1..76d9e88c2c 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp @@ -48,18 +48,19 @@ TEST_F(SelectCommandTest, SelectCommandRequirement) { TestSubsystem requirement3; TestSubsystem requirement4; - InstantCommand command1([] {}, {&requirement1, &requirement2}); - InstantCommand command2([] {}, {&requirement3}); - InstantCommand command3([] {}, {&requirement3, &requirement4}); + auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2}); + auto command2 = cmd::RunOnce([] {}, {&requirement3}); + auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4}); - SelectCommand select([] { return 1; }, std::pair(1, std::move(command1)), - std::pair(2, std::move(command2))); + auto select = + cmd::Select([] { return 1; }, std::pair(1, std::move(command1)), + std::pair(2, std::move(command2))); - scheduler.Schedule(&select); - scheduler.Schedule(&command3); + scheduler.Schedule(select); + scheduler.Schedule(command3); - EXPECT_TRUE(scheduler.IsScheduled(&command3)); - EXPECT_FALSE(scheduler.IsScheduled(&select)); + EXPECT_TRUE(scheduler.IsScheduled(command3)); + EXPECT_FALSE(scheduler.IsScheduled(select)); } class TestableSelectCommand : public SelectCommand { diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp index b8920c733f..3781928ea6 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp @@ -105,15 +105,15 @@ TEST_F(SequentialCommandGroupTest, SequentialGroupCopy) { bool finished = false; - WaitUntilCommand command([&finished] { return finished; }); + auto command = cmd::WaitUntil([&finished] { return finished; }); - SequentialCommandGroup group(command); - scheduler.Schedule(&group); + auto group = cmd::Sequence(std::move(command)); + scheduler.Schedule(group); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(group)); finished = true; scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } TEST_F(SequentialCommandGroupTest, SequentialGroupRequirement) { @@ -124,17 +124,17 @@ TEST_F(SequentialCommandGroupTest, SequentialGroupRequirement) { TestSubsystem requirement3; TestSubsystem requirement4; - InstantCommand command1([] {}, {&requirement1, &requirement2}); - InstantCommand command2([] {}, {&requirement3}); - InstantCommand command3([] {}, {&requirement3, &requirement4}); + auto command1 = cmd::RunOnce([] {}, {&requirement1, &requirement2}); + auto command2 = cmd::RunOnce([] {}, {&requirement3}); + auto command3 = cmd::RunOnce([] {}, {&requirement3, &requirement4}); - SequentialCommandGroup group(std::move(command1), std::move(command2)); + auto group = cmd::Sequence(std::move(command1), std::move(command2)); - scheduler.Schedule(&group); - scheduler.Schedule(&command3); + scheduler.Schedule(group); + scheduler.Schedule(command3); - EXPECT_TRUE(scheduler.IsScheduled(&command3)); - EXPECT_FALSE(scheduler.IsScheduled(&group)); + EXPECT_TRUE(scheduler.IsScheduled(command3)); + EXPECT_FALSE(scheduler.IsScheduled(group)); } INSTANTIATE_MULTI_COMMAND_COMPOSITION_TEST_SUITE(SequentialCommandGroupTest, diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp index 53a2df2a2c..744646d371 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include "CommandTestBase.h" #include "frc2/command/StartEndCommand.h" @@ -13,13 +15,13 @@ TEST_F(StartEndCommandTest, StartEndCommandSchedule) { int counter = 0; - StartEndCommand command([&counter] { counter++; }, [&counter] { counter++; }, - {}); + auto command = + cmd::StartEnd([&counter] { counter++; }, [&counter] { counter++; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); scheduler.Run(); - scheduler.Cancel(&command); + scheduler.Cancel(command); EXPECT_EQ(2, counter); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp index 30eea6b433..a575e65613 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include #include "CommandTestBase.h" @@ -16,14 +18,14 @@ TEST_F(WaitCommandTest, WaitCommandSchedule) { CommandScheduler scheduler = GetScheduler(); - WaitCommand command(100_ms); + auto command = cmd::Wait(100_ms); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(scheduler.IsScheduled(command)); frc::sim::StepTiming(110_ms); scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); frc::sim::ResumeTiming(); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp index 01acc390e1..10b36fe501 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include + #include "CommandTestBase.h" #include "frc2/command/WaitUntilCommand.h" @@ -13,12 +15,12 @@ TEST_F(WaitUntilCommandTest, WaitUntilCommandSchedule) { bool finished = false; - WaitUntilCommand command([&finished] { return finished; }); + auto command = cmd::WaitUntil([&finished] { return finished; }); - scheduler.Schedule(&command); + scheduler.Schedule(command); scheduler.Run(); - EXPECT_TRUE(scheduler.IsScheduled(&command)); + EXPECT_TRUE(scheduler.IsScheduled(command)); finished = true; scheduler.Run(); - EXPECT_FALSE(scheduler.IsScheduled(&command)); + EXPECT_FALSE(scheduler.IsScheduled(command)); } diff --git a/wpilibc/src/main/native/include/frc/Errors.h b/wpilibc/src/main/native/include/frc/Errors.h index 8cdb746bf4..3aae3e446d 100644 --- a/wpilibc/src/main/native/include/frc/Errors.h +++ b/wpilibc/src/main/native/include/frc/Errors.h @@ -147,10 +147,10 @@ namespace warn { * * @param[in] format error message format */ -#define FRC_ReportWarning(format, ...) \ - do { \ - ::frc::ReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, \ - format __VA_OPT__(, ) __VA_ARGS__); \ +#define FRC_ReportWarning(format, ...) \ + do { \ + ::frc::ReportError(::frc::warn::Warning, __FILE__, __LINE__, __FUNCTION__, \ + format __VA_OPT__(, ) __VA_ARGS__); \ } while (0) /** @@ -186,7 +186,7 @@ namespace warn { #define FRC_AssertMessage(condition, format, ...) \ do { \ if (!(condition)) { \ - throw ::frc::MakeError(err::AssertionFailure, __FILE__, __LINE__, \ + throw ::frc::MakeError(::frc::err::AssertionFailure, __FILE__, __LINE__, \ __FUNCTION__, format __VA_OPT__(, ) __VA_ARGS__); \ } \ } while (0) diff --git a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h index 04435f0252..af76205ab3 100644 --- a/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h +++ b/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h @@ -122,10 +122,10 @@ class SmartDashboard { static bool PutBoolean(std::string_view keyName, bool value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist * @param key the key - * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue the value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultBoolean(std::string_view key, bool defaultValue); @@ -135,7 +135,7 @@ class SmartDashboard { * If the key is not found, returns the default value. * * @param keyName the key - * @param defaultValue the default value to set if key doesn't exist + * @param defaultValue the default value to return if key doesn't exist * @return the value */ static bool GetBoolean(std::string_view keyName, bool defaultValue); @@ -153,11 +153,11 @@ class SmartDashboard { static bool PutNumber(std::string_view keyName, double value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key The key. - * @param defaultValue The default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue The value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultNumber(std::string_view key, double defaultValue); @@ -167,7 +167,7 @@ class SmartDashboard { * If the key is not found, returns the default value. * * @param keyName the key - * @param defaultValue the default value to set if the key doesn't exist + * @param defaultValue the default value to return if the key doesn't exist * @return the value */ static double GetNumber(std::string_view keyName, double defaultValue); @@ -185,11 +185,11 @@ class SmartDashboard { static bool PutString(std::string_view keyName, std::string_view value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue the value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultString(std::string_view key, std::string_view defaultValue); @@ -200,7 +200,7 @@ class SmartDashboard { * If the key is not found, returns the default value. * * @param keyName the key - * @param defaultValue the default value to set if the key doesn't exist + * @param defaultValue the default value to return if the key doesn't exist * @return the value */ static std::string GetString(std::string_view keyName, @@ -220,11 +220,11 @@ class SmartDashboard { static bool PutBooleanArray(std::string_view key, std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue the value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultBooleanArray(std::string_view key, std::span defaultValue); @@ -261,11 +261,11 @@ class SmartDashboard { std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key The key. - * @param defaultValue The default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue The value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultNumberArray(std::string_view key, std::span defaultValue); @@ -298,11 +298,11 @@ class SmartDashboard { std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key The key. - * @param defaultValue The default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue The value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultStringArray(std::string_view key, std::span defaultValue); @@ -334,11 +334,11 @@ class SmartDashboard { static bool PutRaw(std::string_view key, std::span value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key The key. - * @param defaultValue The default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue The value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultRaw(std::string_view key, std::span defaultValue); @@ -374,11 +374,11 @@ class SmartDashboard { static bool PutValue(std::string_view keyName, const nt::Value& value); /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue The default value to set if key doesn't exist. - * @returns False if the table key exists with a different type + * @param defaultValue The value to set if key doesn't exist. + * @returns True if the table key did not already exist, otherwise False */ static bool SetDefaultValue(std::string_view key, const nt::Value& defaultValue); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 78a8046f3d..4525d6aa65 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -32,10 +32,9 @@ public class TimedRobot extends IterativeRobotBase { * Construct a callback container. * * @param func The callback to run. - * @param startTimeSeconds The common starting point for all callback scheduling in - * microseconds. - * @param periodSeconds The period at which to run the callback in microseconds. - * @param offsetSeconds The offset from the common starting time in microseconds. + * @param startTimeUs The common starting point for all callback scheduling in microseconds. + * @param periodUs The period at which to run the callback in microseconds. + * @param offsetUs The offset from the common starting time in microseconds. */ Callback(Runnable func, long startTimeUs, long periodUs, long offsetUs) { this.func = func; @@ -54,7 +53,7 @@ public class TimedRobot extends IterativeRobotBase { @Override public int hashCode() { - return Double.hashCode(expirationTime); + return Long.hashCode(expirationTime); } @Override diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java index 7da170b84e..81fbaa6c19 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java @@ -196,11 +196,11 @@ public final class SmartDashboard { } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultBoolean(String key, boolean defaultValue) { return getEntry(key).setDefaultBoolean(defaultValue); @@ -231,11 +231,11 @@ public final class SmartDashboard { } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultNumber(String key, double defaultValue) { return getEntry(key).setDefaultDouble(defaultValue); @@ -266,11 +266,11 @@ public final class SmartDashboard { } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultString(String key, String defaultValue) { return getEntry(key).setDefaultString(defaultValue); @@ -312,22 +312,22 @@ public final class SmartDashboard { } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultBooleanArray(String key, boolean[] defaultValue) { return getEntry(key).setDefaultBooleanArray(defaultValue); } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultBooleanArray(String key, Boolean[] defaultValue) { return getEntry(key).setDefaultBooleanArray(defaultValue); @@ -382,22 +382,22 @@ public final class SmartDashboard { } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultNumberArray(String key, double[] defaultValue) { return getEntry(key).setDefaultDoubleArray(defaultValue); } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultNumberArray(String key, Double[] defaultValue) { return getEntry(key).setDefaultNumberArray(defaultValue); @@ -441,11 +441,11 @@ public final class SmartDashboard { } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultStringArray(String key, String[] defaultValue) { return getEntry(key).setDefaultStringArray(defaultValue); @@ -476,11 +476,11 @@ public final class SmartDashboard { } /** - * Gets the current value in the table, setting it if it does not exist. + * Set the value in the table if key does not exist. * * @param key the key - * @param defaultValue the default value to set if key does not exist. - * @return False if the table key exists with a different type + * @param defaultValue the value to set if key does not exist + * @return True if the key did not already exist, otherwise False */ public static boolean setDefaultRaw(String key, byte[] defaultValue) { return getEntry(key).setDefaultRaw(defaultValue); diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index 5cbfc03755..d95496bd10 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -39,8 +39,9 @@ public class LTVUnicycleController { /** * Constructs a linear time-varying unicycle controller with default maximum desired error - * tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum desired control effort of (1 m/s, - * 2 rad/s). + * tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad), default maximum desired control + * effort of (linear velocity = 1 m/s, angular velocity = 2 rad/s), and default maximum Velocity + * of 9 m/s. * * @param dt Discretization timestep in seconds. */ @@ -55,8 +56,11 @@ public class LTVUnicycleController { * href="https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning">https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning * for how to select the tolerances. * - * @param qelems The maximum desired error tolerance for each state. - * @param relems The maximum desired control effort for each input. + *

The default maximum Velocity is 9 m/s. + * + * @param qelems The maximum desired error tolerance for each state (x, y, heading). + * @param relems The maximum desired control effort for each input (linear velocity, angular + * velocity). * @param dt Discretization timestep in seconds. */ public LTVUnicycleController(Vector qelems, Vector relems, double dt) { diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h index 18aa14ce1e..17589409a6 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -31,8 +31,9 @@ class WPILIB_DLLEXPORT LTVUnicycleController { public: /** * Constructs a linear time-varying unicycle controller with default maximum - * desired error tolerances of (0.0625 m, 0.125 m, 2 rad) and default maximum - * desired control effort of (1 m/s, 2 rad/s). + * desired error tolerances of (x = 0.0625 m, y = 0.125 m, heading = 2 rad) + * and default maximum desired control effort of (linear velocity = 1 m/s, + * angular velocity = 2 rad/s). * * @param dt Discretization timestep. */ @@ -46,8 +47,10 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * https://docs.wpilib.org/en/stable/docs/software/advanced-controls/state-space/state-space-intro.html#lqr-tuning * for how to select the tolerances. * - * @param Qelems The maximum desired error tolerance for each state. - * @param Relems The maximum desired control effort for each input. + * @param Qelems The maximum desired error tolerance for each state (x, y, + * heading). + * @param Relems The maximum desired control effort for each input (linear + * velocity, angular velocity). * @param dt Discretization timestep. */ LTVUnicycleController(const wpi::array& Qelems, diff --git a/wpimath/src/main/native/include/frc/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h index ce339db276..b520cb46fd 100644 --- a/wpimath/src/main/native/include/frc/ct_matrix.h +++ b/wpimath/src/main/native/include/frc/ct_matrix.h @@ -334,7 +334,7 @@ class ct_matrix { requires(Rows == 3 && Cols == 3) { // |a b c| - // |d e f| = aei + bfg + cgh - ceg - bdi - afh + // |d e f| = aei + bfg + cdh - ceg - bdi - afh // |g h i| Scalar a = (*this)(0, 0); Scalar b = (*this)(0, 1); @@ -345,7 +345,7 @@ class ct_matrix { Scalar g = (*this)(2, 0); Scalar h = (*this)(2, 1); Scalar i = (*this)(2, 2); - return a * e * i + b * f * g + c * g * h - c * e * g - b * d * i - + return a * e * i + b * f * g + c * d * h - c * e * g - b * d * i - a * f * h; } diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 2c3c94540a..8498128c21 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -77,7 +76,11 @@ class WPILIB_DLLEXPORT Rotation2d { if ((R * R.transpose() - Matrix2d::Identity()).norm() > 1e-9) { throw std::domain_error("Rotation matrix isn't orthogonal"); } - if (gcem::abs(R.determinant() - 1.0) > 1e-9) { + // HACK: Uses ct_matrix instead of for determinant because + // including doubles compilation times on MSVC, even if + // this constructor is unused. MSVC's frontend inefficiently parses + // large headers; GCC and Clang are largely unaffected. + if (gcem::abs(ct_matrix{R}.determinant() - 1.0) > 1e-9) { throw std::domain_error( "Rotation matrix is orthogonal but not special orthogonal"); } diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index f40be9b09c..8d1e975299 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -8,7 +8,6 @@ #include #include -#include #include #include #include @@ -114,7 +113,11 @@ class WPILIB_DLLEXPORT Rotation3d { if ((R * R.transpose() - Matrix3d::Identity()).norm() > 1e-9) { throw std::domain_error("Rotation matrix isn't orthogonal"); } - if (gcem::abs(R.determinant() - 1.0) > 1e-9) { + // HACK: Uses ct_matrix instead of for determinant because + // including doubles compilation times on MSVC, even if + // this constructor is unused. MSVC's frontend inefficiently parses + // large headers; GCC and Clang are largely unaffected. + if (gcem::abs(ct_matrix{R}.determinant() - 1.0) > 1e-9) { throw std::domain_error( "Rotation matrix is orthogonal but not special orthogonal"); }